#import "PerspMat.h"
#import "math.h"

@implementation PerspMat

+new
{
  register float *fpt;
  register short int n = 16;

  self = [super new];
  fpt = mat; while(n--) *fpt++ = 0.;
  mat[0] = mat[5] = mat[10] = mat[15] = 1.;
  return self;
}

+newCenter:(float)cx :(float)cy :(float)cz
{
  register float *fpt;
  register short int n = 16;

  self = [super new];
  fpt = mat; while(n--) *fpt++ = 0.;
  mat[0] = mat[5] = mat[10] = mat[15] = 1.;
  mat[3] = -cx; mat[7] = -cy; mat[11] = -cz;
  return self;
}

-reset
{
  register float *fpt = mat;
  register short int n = 16;

  while(n--) *fpt++ = 0.;
  mat[0] = mat[5] = mat[10] = mat[15] = 1.;
  return self;
}

-(float **)render:(int)n :(float *)x :(float *)y :(float *)z :(float **)results
{
  float h, *u = results[0], *v = results[1], *w = results[2];
  while (n--) {
    h = mat[12] * *x + mat[13] * *y + mat[14] * *z + mat[15];
    *u++ = (mat[0] * *x + mat[1] * *y + mat[2] * *z + mat[3])/h;
    *v++ = (mat[4] * *x + mat[5] * *y + mat[6] * *z + mat[7])/h;
    *w++ = (mat[8] * *x++ + mat[9] * *y++ + mat[10] * *z++ + mat[11])/h;
  }
  return results;
}

-(float *)as_DPSpath:(int)n :(float *)x :(float *)y :(float *)z :(float *)results
{
  float h, *res = results;
  while (n--) {
    h = mat[12] * *x + mat[13] * *y + mat[14] * *z + mat[15];
    *res++ = (mat[0] * *x + mat[1] * *y + mat[2] * *z + mat[3])/h;
    *res++ = (mat[4] * *x++ + mat[5] * *y++ + mat[6] * *z++ + mat[7])/h;
  }
  return results;
}

-(float *)as_DPSpath:(int)n :(float *)x :(float *)y :(float *)z
  :(float *)results offset:(float *)o_set
{
  float h, *res = results;
  while (n--) {
    h = mat[12] * *x + mat[13] * *y + mat[14] * *z + mat[15];
    *res++ = (mat[0] * *x + mat[1] * *y + mat[2] * *z + mat[3])/h - o_set[0];
    *res++ = (mat[4] * *x++ + mat[5] * *y++ + mat[6] * *z++ +
	      mat[7])/h - o_set[1];
  }
  return results;
}

-translate:(float)dx :(float)dy :(float)dz
{
  register float *thisrow = mat, *lastrow = mat + 12;
  register short int n = 4;

  while (n--) *thisrow++ += dx * *lastrow++;
  n = 4; lastrow -= 4; while (n--) *thisrow++ += dy * *lastrow++;
  n = 4; lastrow -= 4; while (n--) *thisrow++ += dz * *lastrow++;
  return self;
}

-scale:(float)sx :(float)sy :(float)sz
{
  register float *thisrow = mat;
  register short int n = 4;

  while(n--) *thisrow++ *= sx;
  n = 4; while (n--) *thisrow++ *= sy;
  n = 4; while (n--) *thisrow++ *= sz;
  return self;
}

-x_rotation:(float)radians
{
  register float cos_angle = cos(radians), sin_angle = sin(radians), temp,
    *thisrow = mat + 4, *otherrow = mat + 8;
  register short int n = 4;

  while (n--) {
    temp = cos_angle * *thisrow - sin_angle * *otherrow;
    *otherrow++ = sin_angle * *thisrow + cos_angle * *otherrow;
    *thisrow++ = temp;
  }
  return self;
}

-x_rotation_cs:(float)cos_angle :(float)sin_angle
{
  register float temp, *thisrow = mat + 4, *otherrow = mat + 8;
  register short int n = 4;

  while (n--) {
    temp = cos_angle * *thisrow - sin_angle * *otherrow;
    *otherrow++ = sin_angle * *thisrow + cos_angle * *otherrow;
    *thisrow++ = temp;
  }
  return self;
}

-y_rotation:(float)radians
{
  register float cos_angle = cos(radians), sin_angle = sin(radians), temp,
    *thisrow = mat, *otherrow = mat + 8;
  register short int n = 4;

  while (n--) {
    temp = cos_angle * *thisrow + sin_angle * *otherrow;
    *otherrow++ = -sin_angle * *thisrow + cos_angle * *otherrow;
    *thisrow++ = temp;
  }
  return self;
}

-y_rotation_cs:(float)cos_angle :(float)sin_angle
{
  register float temp, *thisrow = mat, *otherrow = mat + 8;
  register short int n = 4;

  while (n--) {
    temp = cos_angle * *thisrow + sin_angle * *otherrow;
    *otherrow++ = -sin_angle * *thisrow + cos_angle * *otherrow;
    *thisrow++ = temp;
  }
  return self;
}

-z_rotation:(float)radians
{
  register float cos_angle = cos(radians), sin_angle = sin(radians), temp,
    *thisrow = mat, *otherrow = mat + 4;
  register short int n = 4;

  while (n--) {
    temp = cos_angle * *thisrow - sin_angle * *otherrow;
    *otherrow++ = sin_angle * *thisrow + cos_angle * *otherrow;
    *thisrow++ = temp;
  }
  return self;
}

-z_rotation_cs:(float)cos_angle :(float)sin_angle
{
  register float temp, *thisrow = mat, *otherrow = mat + 4;
  register short int n = 4;

  while (n--) {
    temp = cos_angle * *thisrow - sin_angle * *otherrow;
    *otherrow++ = sin_angle * *thisrow + cos_angle * *otherrow;
    *thisrow++ = temp;
  }
  return self;
}

-perspective:(float)distance
{
  register float inv_d = 1/distance, *otherrow = mat + 8,
    *thisrow = mat + 12;
  register short int n = 4;

  while (n--) {
    *thisrow++ -= inv_d * *otherrow;
    *otherrow++ = 0.;
  }
  return self;
}

-perspective_inv:(float)invdistance
{
  register float inv_d = invdistance, *otherrow = mat + 8,
    *thisrow = mat + 12;
  register short int n = 4;

  while (n--) {
    *thisrow++ -= inv_d * *otherrow;
    *otherrow++ = 0.;
  }
  return self;
}

@end
