1 ////////////////////////////////////////////////////////////////////////////////
2 // Scorched3D (c) 2000-2011
3 //
4 // This file is part of Scorched3D.
5 //
6 // Scorched3D is free software; you can redistribute it and/or modify
7 // it under the terms of the GNU General Public License as published by
8 // the Free Software Foundation; either version 2 of the License, or
9 // (at your option) any later version.
10 //
11 // Scorched3D is distributed in the hope that it will be useful,
12 // but WITHOUT ANY WARRANTY; without even the implied warranty of
13 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 // GNU General Public License for more details.
15 //
16 // You should have received a copy of the GNU General Public License along
17 // with this program; if not, write to the Free Software Foundation, Inc.,
18 // 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
19 ////////////////////////////////////////////////////////////////////////////////
20
21 #include <common/Vector4.h>
22 #include <common/Defines.h>
23
24 // Some maths funnies taken from ODE
25 #define _R(i,j) R[(i)*4+(j)]
26 #define dDOTpq(a,b,p,q) ((a)[0]*(b)[0] + (a)[p]*(b)[q] + (a)[2*(p)]*(b)[2*(q)])
27 #define dDOT41(a,b) dDOTpq(a,b,4,1)
28 #define dDOT(a,b) dDOTpq(a,b,1,1)
29 #define dMULTIPLYOP1_331(A,op,B,C) \
30 do { \
31 (A)[0] op dDOT41((B),(C)); \
32 (A)[1] op dDOT41((B+1),(C)); \
33 (A)[2] op dDOT41((B+2),(C)); \
34 } while(0)
35 #define dMULTIPLY1_331(A,B,C) dMULTIPLYOP1_331(A,=,B,C)
36
37 static Vector4 nullVector;
38
getNullVector()39 Vector4 &Vector4::getNullVector()
40 {
41 nullVector.zero();
42 return nullVector;
43 }
44
setQuatFromAxisAndAngle(Vector & axis,float angle)45 void Vector4::setQuatFromAxisAndAngle(Vector &axis, float angle)
46 {
47 float l = axis[0]*axis[0] + axis[1]*axis[1] + axis[2]*axis[2];
48 if (l > 0.0f)
49 {
50 angle *= 0.5f;
51 V[0] = cosf(angle);
52 l = sinf(angle) * ((1.0f/sqrtf(l)));
53 V[1] = axis[0] * l;
54 V[2] = axis[1] * l;
55 V[3] = axis[2] * l;
56 }
57 else
58 {
59 V[0] = 1.0f;
60 V[1] = 0.0f;
61 V[2] = 0.0f;
62 V[3] = 0.0f;
63 }
64 }
65
66 // derived from equations in "An Introduction
67 // to Physically Based Modeling: Rigid Body Simulation - 1: Unconstrained
68 // Rigid Body Dynamics" by David Baraff, Robotics Institute, Carnegie Mellon
69 // University, 1997.
getRotationMatrix(float * R)70 void Vector4::getRotationMatrix(float *R)
71 {
72 // q = (s,vx,vy,vz)
73 float qq1 = 2*V[1]*V[1];
74 float qq2 = 2*V[2]*V[2];
75 float qq3 = 2*V[3]*V[3];
76 _R(0,0) = 1 - qq2 - qq3;
77 _R(0,1) = 2*(V[1]*V[2] - V[0]*V[3]);
78 _R(0,2) = 2*(V[1]*V[3] + V[0]*V[2]);
79 _R(0,3) = 0.0f;
80 _R(1,0) = 2*(V[1]*V[2] + V[0]*V[3]);
81 _R(1,1) = 1 - qq1 - qq3;
82 _R(1,2) = 2*(V[2]*V[3] - V[0]*V[1]);
83 _R(1,3) = 0.0f;
84 _R(2,0) = 2*(V[1]*V[3] - V[0]*V[2]);
85 _R(2,1) = 2*(V[2]*V[3] + V[0]*V[1]);
86 _R(2,2) = 1 - qq1 - qq2;
87 _R(2,3) = 0.0f;
88 }
89
getOpenGLRotationMatrix(float * rotMatrix)90 void Vector4::getOpenGLRotationMatrix(float *rotMatrix)
91 {
92 static float matrix[12];
93 getRotationMatrix(matrix);
94
95 rotMatrix[0] = matrix[0];
96 rotMatrix[1] = matrix[4];
97 rotMatrix[2] = matrix[8];
98 rotMatrix[4] = matrix[1];
99 rotMatrix[5] = matrix[5];
100 rotMatrix[6] = matrix[9];
101 rotMatrix[8] = matrix[2];
102 rotMatrix[9] = matrix[6];
103 rotMatrix[10] = matrix[10];
104 rotMatrix[3] = rotMatrix[7] = rotMatrix[11] = 0.0;
105 rotMatrix[15] = 1.0;
106 rotMatrix[12] = rotMatrix[13] = rotMatrix[14] = 0.0;
107 }
108
Normalize()109 void Vector4::Normalize()
110 {
111 float l = dDOT(V,V) + V[3] * V[3];
112 if (l > 0)
113 {
114 l = 1.0f / sqrtf(l);
115 V[0] *= l;
116 V[1] *= l;
117 V[2] *= l;
118 V[3] *= l;
119 }
120 else
121 {
122 V[0] = 1;
123 V[1] = 0;
124 V[2] = 0;
125 V[3] = 0;
126 }
127 }
128
getRelativeVector(Vector & r,Vector & p)129 void Vector4::getRelativeVector(Vector &r, Vector &p)
130 {
131 static float matrix[12];
132 getRotationMatrix(matrix);
133
134 float *result = r;
135 float *position = p;
136 dMULTIPLY1_331(result, matrix, position);
137 }
138
dDQfromW(Vector4 & dq,Vector & w,Vector4 & q)139 void Vector4::dDQfromW(Vector4 &dq, Vector &w, Vector4 &q)
140 {
141 dq[0] = 0.5f *(- w[0]*q[1] - w[1]*q[2] - w[2]*q[3]);
142 dq[1] = 0.5f *( w[0]*q[0] + w[1]*q[3] - w[2]*q[2]);
143 dq[2] = 0.5f *(- w[0]*q[3] + w[1]*q[0] + w[2]*q[1]);
144 dq[3] = 0.5f *( w[0]*q[2] - w[1]*q[1] + w[2]*q[0]);
145 }
146