trixd.c

来自「细胞自动机的一个源代码」· C语言 代码 · 共 477 行

/*are for kids!
  matrix library...
  matrix[ROW][COLUMN]
  vectors are defined to be column vectors
  if we have points in a 3x3 matrix, they are in the COLUMNS of the matrix
  matricies should always be thought of as being multiplied on the LEFT of the
    vector...i.e. A*v
  thank you

  internal rep. of matricies is

  [row]  [column]
  *---->x x x x
  *---->x x x x
  *---->x x x x
  *---->x x x x 
  *---->x x x x
  would represent a 5x4 matrix

  makedyntrixd     - constructor
  destroydyntrixd  - destructor

  printtrixd - print the matrix to stdout, for debuggin'
  trixmultd  - a*b --> out
  trixrottz  - a rotated by theta --> b
  trixrotty  - a rotated by theta --> b
  trixrottx  - a rotated by theta --> b

  trixtransd  - translates trix by double vec
  trixtrans3d - blah

  lengthsqd -length squared of trixpoint
  lengthd   -length of trix point

  length2d  -length between two trixd points
  lengthsq2d-length between two trixd points squared

  closestpoint
*/

#include "trixd.h"

void printtrixd(trixd *m) {
  int i, j;
  for (i=0; i<(m->row); i++) {
    for (j=0; j<(m->column); j++) {
      printf("%f ", (float)((m->d)[i][j]));
    }
    printf("\n");
  }
}

/*mat not to be allocated before calling this function (this function
  will allocate space for it).  mat will be a rowXcolumn matrix*/
trixd *makedyntrixd(int row, int column) {
  int i, j, k;
  trixd *m;

  m = (trixd *)malloc(sizeof(trixd));
  m->d = malloc( (sizeof(double *)*row) + (sizeof(double)*row*column));

  /*ansi c should be smart enough to know that i'm referencing double * stuff*/
  /*not sure, but this code probably isn't that portable (but its still cool)*/
  for (i=0; i<row; i++) {
    j = (int)m->d;
    j+=row*sizeof(double *);
    j+=i*column*sizeof(double);

    m->d[i]=(double *)j;

    /*
    m->d[i] = (double *)m->d + row + i*column;
    */
  }

  for (i=0; i<row; i++) {
    for (j=0; j<column; j++) {
      m->d[i][j]=(double)((i*column)+j);
    }
  }

  m->row = row;
  m->column = column;
  m->f = NULL;
  m->fpos = NULL;

  m->dpos = m->d[m->row];
  m->dpos = &(m->d[0][0]);

  /*  m->dpos = m->d + row;/*this won't work as of right now...change later*/
  return(m);
}

int destroydyntrixd(trixd *m) {
  int i, j;
  free(m->d);
  free(m);
  return(0);
}

/*translate inp[][3] by vec[3], put in oot*/
/*void trans(double inp[][3], double oot[][3], double t[3], int n) {*/
void trixtrans3d(trixd *inp, double x, double y, double z) {
  int i, j;

  for (i=0; i<(inp->column); i++) {
    inp->d[0][i]+=x;
    inp->d[1][i]+=y;
    inp->d[2][i]+=z;
  }
}

/*
void trixtransd(trixd *inp, double *v) {
  int i, j;

  for (i=0; i<(inp->column); i++) {
    inp->d[0][i]+=v[0];
    inp->d[1][i]+=v[1];
    inp->d[2][i]+=v[2];
  }
}
*/

void trixtransd(trixd *inp, trixd *outp, double x, double y, double z) {
  int i, j, k;
  static trixd *tem = NULL;

  tem = ( tem ? tem : makedyntrixd(4, 4) );

  for (i=0; i<4; i++) {
    for (j=0; j<4; j++) {
      tem->d[i][j]=((i==j)?1.0:0.0);
    }
  }

  tem->d[0][3] = x;
  tem->d[1][3] = y;
  tem->d[2][3] = z;

  /*
  printf("trixtransd:\n");
  printf("  tem\n");
  printtrixd(tem);
  printf("  inp\n");
  printtrixd(inp);
  */

  trixmultd(tem, inp, outp);

  //printf("  outp\n");
  //printtrixd(outp);

}

trixd *trixmultd(trixd *l, trixd *r, trixd *out) {

  int i, j, k;

  for (i=0; i<(l->row); i++) {
    for (j=0; j<(r->column); j++) {
      out->d[i][j]=0.0;
      for (k=0; k<(l->column); k++) {
	out->d[i][j]+= (l->d[i][k])*(r->d[k][j]);
      }
    }
  }
  return(out);

}

void trixrottzd(trixd *inp, trixd *outp, double theta) {
  int i, j, k;

  static trixd *rd=NULL;

  rd = (rd ? rd : makedyntrixd(4, 4));

  rd->d[0][0]=cos(theta*PI/180.0);
  rd->d[0][1]=-sin(theta*PI/180.0);
  rd->d[1][0]=-(rd->d[0][1]);
  rd->d[1][1]=rd->d[0][0];

  rd->d[2][0]=rd->d[2][1]=rd->d[0][2]=rd->d[1][2]=0.0;
  rd->d[2][2]=1.0;

  rd->d[0][3]=0.0;
  rd->d[1][3]=0.0;
  rd->d[2][3]=0.0;

  rd->d[3][0]=0.0;
  rd->d[3][1]=0.0;
  rd->d[3][2]=0.0;
  rd->d[3][3]=1.0;

  trixmultd(rd, inp, outp);

  /*  destroydyntrixd(rd); */
}

void trixrottyd(trixd *inp, trixd *outp, double theta) {
  int i, j, k;

  static trixd *rd=NULL;

  rd = (rd ? rd : makedyntrixd(4, 4));

  rd->d[0][0]=cos(theta*PI/180.0);
  rd->d[0][2]=sin(theta*PI/180.0);
  rd->d[2][0]=-rd->d[0][2];
  rd->d[2][2]=rd->d[0][0];
  rd->d[1][1]=1.0;
  rd->d[1][0]=rd->d[1][2]=rd->d[0][1]=rd->d[2][1]=0.0;

  rd->d[0][3]=0.0;
  rd->d[1][3]=0.0;
  rd->d[2][3]=0.0;

  rd->d[3][0]=0.0;
  rd->d[3][1]=0.0;
  rd->d[3][2]=0.0;
  rd->d[3][3]=1.0;
  trixmultd(rd, inp, outp);

  /*  destroydyntrixd(rd); */

}

void trixrottxd(trixd *inp, trixd *outp, double theta) {
  int i, j, k;

  static trixd *rd=NULL;

  rd = (rd ? rd : makedyntrixd(4, 4));

  rd->d[0][0]=1.0;
  rd->d[0][1]=rd->d[0][2]=rd->d[1][0]=rd->d[2][0]=0.0;
  rd->d[1][1]=rd->d[2][2]=cos(theta*PI/180.0);
  rd->d[2][1]=sin(theta*PI/180.0);
  rd->d[1][2]=-rd->d[2][1];

  rd->d[0][3]=0.0;
  rd->d[1][3]=0.0;
  rd->d[2][3]=0.0;

  rd->d[3][0]=0.0;
  rd->d[3][1]=0.0;
  rd->d[3][2]=0.0;
  rd->d[3][3]=1.0;

  trixmultd(rd, inp, outp);

  /*  destroydyntrixd(rd); */

}

void trixrottd(trixd *inp, trixd *outp, double theta,
	       double x, double y, double z) {
  int i, j, k;
  static trixd *rd=NULL;
  double c, s, omc;
  double len;

  rd = (rd ? rd : makedyntrixd(4, 4));

  c = cos(theta*PI/180.0);
  s = sin(theta*PI/180.0);
  omc = 1.0 - c;

  len = sqrt((x*x)+(y*y)+(z*z));
  if ( (len<.99999) || (len>1.000001) ) {
    x/=len;
    y/=len;
    z/=len;
  }

  rd->d[0][0]=(x*x*omc)+c;
  rd->d[0][1]=(x*y*omc)-(z*s);
  rd->d[0][2]=(x*z*omc)+(y*s);
  rd->d[0][3]=0.0;

  rd->d[1][0]=(y*x*omc)+(z*s);
  rd->d[1][1]=(y*y*omc)+c;
  rd->d[1][2]=(y*z*omc)-(x*s);
  rd->d[1][3]=0.0;

  rd->d[2][0]=(x*z*omc)-(y*s);
  rd->d[2][1]=(y*z*omc)+(x*s);
  rd->d[2][2]=(z*z*omc)+c;
  rd->d[2][3]=0.0;

  rd->d[3][0]=0.0;
  rd->d[3][1]=0.0;
  rd->d[3][2]=0.0;
  rd->d[3][3]=1.0;

  trixmultd(rd, inp, outp);

}

/*rotate nx3  inp matrix by theta (in degrees), put it oot*/
void rottz(double inp[][3], double oot[][3], double theta, int n) {

  int i, j, k;
  double rot[3][3], t;

  rot[0][0]=cos(theta*PI/180.0);
  rot[0][1]=-sin(theta*PI/180.0);
  rot[1][0]=-rot[0][1];
  rot[1][1]=rot[0][0];
  rot[2][0]=rot[2][1]=rot[0][2]=rot[1][2]=0.0;
  rot[2][2]=1.0;

  for (i=0; i<n; i++) {
    for (j=0; j<3; j++) {
      t=0.0;
      for (k=0; k<3; k++) {
	t+=inp[i][k]*rot[j][k];
      }
      oot[i][j]=t;
    }
  }
}

void rotty(double inp[][3], double oot[][3], double theta, int n) {

  int i, j, k;

  double rot[3][3], t;

  rot[0][0]=cos(theta*PI/180.0);
  rot[0][2]=sin(theta*PI/180.0);
  rot[2][0]=-rot[0][2];
  rot[2][2]=rot[0][0];
  rot[1][1]=1.0;
  rot[1][0]=rot[1][2]=rot[0][1]=rot[2][1]=0.0;

  for (i=0; i<n; i++) {
    for (j=0; j<3; j++) {
      t=0.0;
      for (k=0; k<3; k++) {
	t+=inp[i][k]*rot[j][k];
      }
      oot[i][j]=t;
    }
  }
}

void rottx(double inp[][3], double oot[][3], double theta, int n) {

  int i, j, k;

  double rot[3][3], t;

  rot[0][0]=1.0;
  rot[0][1]=rot[0][2]=rot[1][0]=rot[2][0]=0.0;
  rot[1][1]=rot[2][2]=cos(theta*PI/180.0);
  rot[2][1]=sin(theta*PI/180.0);
  rot[1][2]=-rot[2][1];

    for (i=0; i<n; i++) {
    for (j=0; j<3; j++) {
      t=0.0;
      for (k=0; k<3; k++) {
	t+=inp[i][k]*rot[j][k];
      }
      oot[i][j]=t;
    }
  }
}

/*find point on line of p1 to p2, closest to p3*/
int trixclosepnt(trixd *p1, trixd *p2, trixd *p3, trixd *oot) {

  int i, j, k;
  double tem, len;
  double v1[3], v2[3];

  v1[0] = p2->d[0][0] - p1->d[0][0];  
  v1[1] = p2->d[1][0] - p1->d[1][0];
  v1[2] = p2->d[2][0] - p1->d[2][0];

  v2[0] = p3->d[0][0] - p1->d[0][0];
  v2[1] = p3->d[1][0] - p1->d[1][0];
  v2[2] = p3->d[2][0] - p1->d[2][0];


  tem=len=0.0;
  for (i=0; i<3; i++) {
    tem+=v1[i]*v2[i];
    len+=v1[i]*v1[i];
  }

  for (i=0; i<3; i++) {
    v1[i]*=tem/len;
    v1[i]+=p1->d[i][0];
    oot->d[i][0]=v1[i];
  }

  return(1);

}

double trixlensq2d(trixd *p1, trixd *p2) {
  int i;
  double temp, v[3];
  temp=0.0;
  for (i=0; i<3; i++) {
    v[i] = p1->d[i][0] - p2->d[i][0];
    temp+=v[i]*v[i];
  }
  return(temp);
}

double trixlen2d(trixd *p1, trixd *p2) {
  int i;
  double temp, v[3];
  temp=0.0;
  for (i=0; i<3; i++) {
    v[i] = p1->d[i][0] - p2->d[i][0];
    temp+=v[i]*v[i];
  }
  return(sqrt(temp));
}

double trixlensqd(trixd *p1) {
  return( ((p1->d[0][0])*(p1->d[0][0])) + ((p1->d[1][0])*(p1->d[1][0])) + ((p1->d[2][0])*(p1->d[2][0]))); 
}


double trixlend(trixd *p1) {
  return( sqrt( ((p1->d[0][0])*(p1->d[0][0])) + ((p1->d[1][0])*(p1->d[1][0])) + ((p1->d[2][0])*(p1->d[2][0]))));
}

/*
int main(void) {
  trixd *p1, *p2, *p3;
  trixd *out;

  p1 = makedyntrixd(4, 1);
  p2 = makedyntrixd(4, 1);
  p3 = makedyntrixd(3, 1);

  out = makedyntrixd(3, 1);

  p1->d[0][0]=1.0;
  p1->d[1][0]=1.0;
  p1->d[2][0]=0.0;
  p1->d[3][0]=1.0;
  
  p2->d[0][0]=4.0;
  p2->d[1][0]=2.0;
  p2->d[2][0]=0.0;
  p2->d[3][0]=1.0;

  p3->d[0][0]=3.0;
  p3->d[1][0]=3.0;
  p3->d[2][0]=0.0;

  trixclosepnt(p1, p2, p3, out);

  printtrixd(out);

  printf("p1: %f %f %f\n", p1->d[0][0], p1->d[1][0], p1->d[2][0]);
  trixrottd(p1, p2, 90.0, 1.0, 0.0, 0.0);
  printf("p2: %f %f %f\n", p2->d[0][0], p2->d[1][0], p2->d[2][0]);
  trixrottd(p2, p1, 90.0, 1.0, 0.0, 0.0);
  printf("p1: %f %f %f\n", p1->d[0][0], p1->d[1][0], p1->d[2][0]);

  trixrottd(p1, p2, 90.0, 1.0, 0.0, 0.0);
  printf("p2: %f %f %f\n", p2->d[0][0], p2->d[1][0], p2->d[2][0]);
  trixrottd(p2, p1, 90.0, 1.0, 0.0, 0.0);
  printf("p1: %f %f %f\n", p1->d[0][0], p1->d[1][0], p1->d[2][0]);

}
*/