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