1 #include "float_math.h"
10 /*----------------------------------------------------------------------
11 Copyright (c) 2004 Open Dynamics Framework Group
15 Redistribution and use in source and binary forms, with or without modification, are permitted provided
16 that the following conditions are met:
18 Redistributions of source code must retain the above copyright notice, this list of conditions
19 and the following disclaimer.
21 Redistributions in binary form must reproduce the above copyright notice,
22 this list of conditions and the following disclaimer in the documentation
23 and/or other materials provided with the distribution.
25 Neither the name of the Open Dynamics Framework Group nor the names of its contributors may
26 be used to endorse or promote products derived from this software without specific prior written permission.
28 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' AND ANY EXPRESS OR IMPLIED WARRANTIES,
29 INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
30 DISCLAIMED. IN NO EVENT SHALL THE INTEL OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
31 EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
32 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
33 IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
34 THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
35 -----------------------------------------------------------------------*/
37 // http://codesuppository.blogspot.com
39 // mailto: jratcliff@infiniplex.net
41 // http://www.amillionpixels.us
44 void fm_inverseRT(const float *matrix,const float *pos,float *t) // inverse rotate translate the point.
47 float _x = pos[0] - matrix[3*4+0];
48 float _y = pos[1] - matrix[3*4+1];
49 float _z = pos[2] - matrix[3*4+2];
51 // Multiply inverse-translated source vector by inverted rotation transform
53 t[0] = (matrix[0*4+0] * _x) + (matrix[0*4+1] * _y) + (matrix[0*4+2] * _z);
54 t[1] = (matrix[1*4+0] * _x) + (matrix[1*4+1] * _y) + (matrix[1*4+2] * _z);
55 t[2] = (matrix[2*4+0] * _x) + (matrix[2*4+1] * _y) + (matrix[2*4+2] * _z);
60 void fm_identity(float *matrix) // set 4x4 matrix to identity.
85 void fm_eulerMatrix(float ax,float ay,float az,float *matrix) // convert euler (in radians) to a dest 4x4 matrix (translation set to zero)
88 fm_eulerToQuat(ax,ay,az,quat);
89 fm_quatToMatrix(quat,matrix);
92 void fm_getAABB(unsigned int vcount,const float *points,unsigned int pstride,float *bmin,float *bmax)
95 const unsigned char *source = (const unsigned char *) points;
106 for (unsigned int i=1; i<vcount; i++)
109 const float *p = (const float *) source;
111 if ( p[0] < bmin[0] ) bmin[0] = p[0];
112 if ( p[1] < bmin[1] ) bmin[1] = p[1];
113 if ( p[2] < bmin[2] ) bmin[2] = p[2];
115 if ( p[0] > bmax[0] ) bmax[0] = p[0];
116 if ( p[1] > bmax[1] ) bmax[1] = p[1];
117 if ( p[2] > bmax[2] ) bmax[2] = p[2];
123 void fm_eulerToQuat(float roll,float pitch,float yaw,float *quat) // convert euler angles to quaternion.
129 float cr = cosf(roll);
130 float cp = cosf(pitch);
131 float cy = cosf(yaw);
133 float sr = sinf(roll);
134 float sp = sinf(pitch);
135 float sy = sinf(yaw);
137 float cpcy = cp * cy;
138 float spsy = sp * sy;
139 float spcy = sp * cy;
140 float cpsy = cp * sy;
142 quat[0] = ( sr * cpcy - cr * spsy);
143 quat[1] = ( cr * spcy + sr * cpsy);
144 quat[2] = ( cr * cpsy - sr * spcy);
145 quat[3] = cr * cpcy + sr * spsy;
148 void fm_quatToMatrix(const float *quat,float *matrix) // convert quaterinion rotation to matrix, zeros out the translation component.
151 float xx = quat[0]*quat[0];
152 float yy = quat[1]*quat[1];
153 float zz = quat[2]*quat[2];
154 float xy = quat[0]*quat[1];
155 float xz = quat[0]*quat[2];
156 float yz = quat[1]*quat[2];
157 float wx = quat[3]*quat[0];
158 float wy = quat[3]*quat[1];
159 float wz = quat[3]*quat[2];
161 matrix[0*4+0] = 1 - 2 * ( yy + zz );
162 matrix[1*4+0] = 2 * ( xy - wz );
163 matrix[2*4+0] = 2 * ( xz + wy );
165 matrix[0*4+1] = 2 * ( xy + wz );
166 matrix[1*4+1] = 1 - 2 * ( xx + zz );
167 matrix[2*4+1] = 2 * ( yz - wx );
169 matrix[0*4+2] = 2 * ( xz - wy );
170 matrix[1*4+2] = 2 * ( yz + wx );
171 matrix[2*4+2] = 1 - 2 * ( xx + yy );
173 matrix[3*4+0] = matrix[3*4+1] = matrix[3*4+2] = 0.0f;
174 matrix[0*4+3] = matrix[1*4+3] = matrix[2*4+3] = 0.0f;
175 matrix[3*4+3] = 1.0f;
180 void fm_quatRotate(const float *quat,const float *v,float *r) // rotate a vector directly by a quaternion.
184 left[0] = quat[3]*v[0] + quat[1]*v[2] - v[1]*quat[2];
185 left[1] = quat[3]*v[1] + quat[2]*v[0] - v[2]*quat[0];
186 left[2] = quat[3]*v[2] + quat[0]*v[1] - v[0]*quat[1];
187 left[3] = - quat[0]*v[0] - quat[1]*v[1] - quat[2]*v[2];
189 r[0] = (left[3]*-quat[0]) + (quat[3]*left[0]) + (left[1]*-quat[2]) - (-quat[1]*left[2]);
190 r[1] = (left[3]*-quat[1]) + (quat[3]*left[1]) + (left[2]*-quat[0]) - (-quat[2]*left[0]);
191 r[2] = (left[3]*-quat[2]) + (quat[3]*left[2]) + (left[0]*-quat[1]) - (-quat[0]*left[1]);
196 void fm_getTranslation(const float *matrix,float *t)
198 t[0] = matrix[3*4+0];
199 t[1] = matrix[3*4+1];
200 t[2] = matrix[3*4+2];
203 void fm_matrixToQuat(const float *matrix,float *quat) // convert the 3x3 portion of a 4x4 matrix into a quaterion as x,y,z,w
206 float tr = matrix[0*4+0] + matrix[1*4+1] + matrix[2*4+2];
208 // check the diagonal
212 float s = (float) sqrt ( (double) (tr + 1.0f) );
215 quat[0] = (matrix[1*4+2] - matrix[2*4+1]) * s;
216 quat[1] = (matrix[2*4+0] - matrix[0*4+2]) * s;
217 quat[2] = (matrix[0*4+1] - matrix[1*4+0]) * s;
222 // diagonal is negative
223 int nxt[3] = {1, 2, 0};
228 if (matrix[1*4+1] > matrix[0*4+0]) i = 1;
229 if (matrix[2*4+2] > matrix[i*4+i]) i = 2;
234 float s = sqrtf ( ((matrix[i*4+i] - (matrix[j*4+j] + matrix[k*4+k])) + 1.0f) );
238 if (s != 0.0f ) s = 0.5f / s;
240 qa[3] = (matrix[j*4+k] - matrix[k*4+j]) * s;
241 qa[j] = (matrix[i*4+j] + matrix[j*4+i]) * s;
242 qa[k] = (matrix[i*4+k] + matrix[k*4+i]) * s;
254 float fm_sphereVolume(float radius) // return's the volume of a sphere of this radius (4/3 PI * R cubed )
256 return (4.0f / 3.0f ) * FM_PI * radius * radius * radius;