applications/utilities/surface/surfaceCoarsen/bunnylod/vector.C

   1 \r
   2 #include <stdio.h>\r
   3 #include <math.h>\r
   4 #include <assert.h>\r
   5 \r
   6 #include "vector.h"\r
   7 \r
   8 float  sqr(float a) {return a*a;}\r
   9 \r
  10 // vector (floating point) implementation\r
  11 \r
  12 float magnitude(Vector v) {\r
  13     return float(sqrt(sqr(v.x) + sqr( v.y)+ sqr(v.z)));\r
  14 }\r
  15 Vector normalize(Vector v) {\r
  16     float d=magnitude(v);\r
  17     if (d==0) {\r
  18                 printf("Cant normalize ZERO vector\n");\r
  19                 assert(0);\r
  20                 d=0.1f;\r
  21         }\r
  22     v.x/=d;\r
  23     v.y/=d;\r
  24     v.z/=d;\r
  25     return v;\r
  26 }\r
  27 \r
  28 Vector operator+(Vector v1,Vector v2) {return Vector(v1.x+v2.x,v1.y+v2.y,v1.z+v2.z);}\r
  29 Vector operator-(Vector v1,Vector v2) {return Vector(v1.x-v2.x,v1.y-v2.y,v1.z-v2.z);}\r
  30 Vector operator-(Vector v)            {return Vector(-v.x,-v.y,-v.z);}\r
  31 Vector operator*(Vector v1,float s)   {return Vector(v1.x*s,v1.y*s,v1.z*s);}\r
  32 Vector operator*(float s, Vector v1)  {return Vector(v1.x*s,v1.y*s,v1.z*s);}\r
  33 Vector operator/(Vector v1,float s)   {return v1*(1.0f/s);}\r
  34 float  operator^(Vector v1,Vector v2) {return v1.x*v2.x + v1.y*v2.y + v1.z*v2.z;}\r
  35 Vector operator*(Vector v1,Vector v2) {\r
  36     return Vector(\r
  37                 v1.y * v2.z - v1.z*v2.y,\r
  38                 v1.z * v2.x - v1.x*v2.z,\r
  39                 v1.x * v2.y - v1.y*v2.x);\r
  40 }\r
  41 Vector planelineintersection(Vector n,float d,Vector p1,Vector p2){\r
  42         // returns the point where the line p1-p2 intersects the plane n&d\r
  43         Vector dif  = p2-p1;\r
  44         float dn= n^dif;\r
  45         float t = -(d+(n^p1) )/dn;\r
  46         return p1 + (dif*t);\r
  47 }\r
  48 int concurrent(Vector a,Vector b) {\r
  49         return(a.x==b.x && a.y==b.y && a.z==b.z);\r
  50 }\r
  51 \r
  52 \r
  53 // Matrix Implementation\r
  54 matrix transpose(matrix m) {\r
  55         return matrix(  Vector(m.x.x,m.y.x,m.z.x),\r
  56                                         Vector(m.x.y,m.y.y,m.z.y),\r
  57                                         Vector(m.x.z,m.y.z,m.z.z));\r
  58 }\r
  59 Vector operator*(matrix m,Vector v){\r
  60         m=transpose(m); // since column ordered\r
  61         return Vector(m.x^v,m.y^v,m.z^v);\r
  62 }\r
  63 matrix operator*(matrix m1,matrix m2){\r
  64         m1=transpose(m1);\r
  65         return matrix(m1*m2.x,m1*m2.y,m1*m2.z);\r
  66 }\r
  67 \r
  68 //Quaternion Implementation    \r
  69 Quaternion operator*(Quaternion a,Quaternion b) {\r
  70         Quaternion c;\r
  71         c.r = a.r*b.r - a.x*b.x - a.y*b.y - a.z*b.z; \r
  72         c.x = a.r*b.x + a.x*b.r + a.y*b.z - a.z*b.y; \r
  73         c.y = a.r*b.y - a.x*b.z + a.y*b.r + a.z*b.x; \r
  74         c.z = a.r*b.z + a.x*b.y - a.y*b.x + a.z*b.r; \r
  75         return c;\r
  76 }\r
  77 Quaternion operator-(Quaternion q) {\r
  78         return Quaternion(q.r*-1,q.x,q.y,q.z);\r
  79 }\r
  80 Quaternion operator*(Quaternion a,float b) {\r
  81         return Quaternion(a.r*b, a.x*b, a.y*b, a.z*b);\r
  82 }\r
  83 Vector operator*(Quaternion q,Vector v) {\r
  84         return q.getmatrix() * v;\r
  85 }\r
  86 Vector operator*(Vector v,Quaternion q){\r
  87         assert(0);  // must multiply with the quat on the left\r
  88         return Vector(0.0f,0.0f,0.0f);\r
  89 }\r
  90 \r
  91 Quaternion operator+(Quaternion a,Quaternion b) {\r
  92         return Quaternion(a.r+b.r, a.x+b.x, a.y+b.y, a.z+b.z);\r
  93 }\r
  94 float operator^(Quaternion a,Quaternion b) {\r
  95         return  (a.r*b.r + a.x*b.x + a.y*b.y + a.z*b.z); \r
  96 }\r
  97 Quaternion slerp(Quaternion a,Quaternion b,float interp){\r
  98         if((a^b) <0.0) {\r
  99                 a.r=-a.r;\r
 100                 a.x=-a.x;\r
 101                 a.y=-a.y;\r
 102                 a.z=-a.z;\r
 103         }\r
 104         float theta = float(acos(a^b));\r
 105         if(theta==0.0f) { return(a);}\r
 106         return a*float(sin(theta-interp*theta)/sin(theta)) + b*float(sin(interp*theta)/sin(theta));\r
 107 }\r
 108 \r