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

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