Extras/ConvexDecomposition/float_math.cpp

   1 #include "float_math.h"
   2
   3 #include <stdio.h>
   4 #include <stdlib.h>
   5 #include <string.h>
   6 #include <assert.h>
   7 #include <math.h>
   8
   9
  10 /*----------------------------------------------------------------------
  11                 Copyright (c) 2004 Open Dynamics Framework Group
  12                                         www.physicstools.org
  13                 All rights reserved.
  14
  15                 Redistribution and use in source and binary forms, with or without modification, are permitted provided
  16                 that the following conditions are met:
  17
  18                 Redistributions of source code must retain the above copyright notice, this list of conditions
  19                 and the following disclaimer.
  20
  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.
  24
  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.
  27
  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 -----------------------------------------------------------------------*/
  36
  37 // http://codesuppository.blogspot.com
  38 //
  39 // mailto: jratcliff@infiniplex.net
  40 //
  41 // http://www.amillionpixels.us
  42 //
  43
  44 void fm_inverseRT(const float *matrix,const float *pos,float *t) // inverse rotate translate the point.
  45 {
  46
  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];
  50
  51         // Multiply inverse-translated source vector by inverted rotation transform
  52
  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);
  56
  57 }
  58
  59
  60 void fm_identity(float *matrix) // set 4x4 matrix to identity.
  61 {
  62         matrix[0*4+0] = 1;
  63         matrix[1*4+1] = 1;
  64         matrix[2*4+2] = 1;
  65         matrix[3*4+3] = 1;
  66
  67         matrix[1*4+0] = 0;
  68         matrix[2*4+0] = 0;
  69         matrix[3*4+0] = 0;
  70
  71         matrix[0*4+1] = 0;
  72         matrix[2*4+1] = 0;
  73         matrix[3*4+1] = 0;
  74
  75         matrix[0*4+2] = 0;
  76         matrix[1*4+2] = 0;
  77         matrix[3*4+2] = 0;
  78
  79         matrix[0*4+3] = 0;
  80         matrix[1*4+3] = 0;
  81         matrix[2*4+3] = 0;
  82
  83 }
  84
  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)
  86 {
  87   float quat[4];
  88   fm_eulerToQuat(ax,ay,az,quat);
  89   fm_quatToMatrix(quat,matrix);
  90 }
  91
  92 void fm_getAABB(unsigned int vcount,const float *points,unsigned int pstride,float *bmin,float *bmax)
  93 {
  94
  95   const unsigned char *source = (const unsigned char *) points;
  96
  97         bmin[0] = points[0];
  98         bmin[1] = points[1];
  99         bmin[2] = points[2];
 100
 101         bmax[0] = points[0];
 102         bmax[1] = points[1];
 103         bmax[2] = points[2];
 104
 105
 106   for (unsigned int i=1; i<vcount; i++)
 107   {
 108         source+=pstride;
 109         const float *p = (const float *) source;
 110
 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];
 114
 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];
 118
 119   }
 120 }
 121
 122
 123 void fm_eulerToQuat(float roll,float pitch,float yaw,float *quat) // convert euler angles to quaternion.
 124 {
 125         roll  *= 0.5f;
 126         pitch *= 0.5f;
 127         yaw   *= 0.5f;
 128
 129         float cr = cosf(roll);
 130         float cp = cosf(pitch);
 131         float cy = cosf(yaw);
 132
 133         float sr = sinf(roll);
 134         float sp = sinf(pitch);
 135         float sy = sinf(yaw);
 136
 137         float cpcy = cp * cy;
 138         float spsy = sp * sy;
 139         float spcy = sp * cy;
 140         float cpsy = cp * sy;
 141
 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;
 146 }
 147
 148 void fm_quatToMatrix(const float *quat,float *matrix) // convert quaterinion rotation to matrix, zeros out the translation component.
 149 {
 150
 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];
 160
 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 );
 164
 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 );
 168
 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 );
 172
 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;
 176
 177 }
 178
 179
 180 void fm_quatRotate(const float *quat,const float *v,float *r) // rotate a vector directly by a quaternion.
 181 {
 182   float left[4];
 183
 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];
 188
 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]);
 192
 193 }
 194
 195
 196 void fm_getTranslation(const float *matrix,float *t)
 197 {
 198         t[0] = matrix[3*4+0];
 199         t[1] = matrix[3*4+1];
 200         t[2] = matrix[3*4+2];
 201 }
 202
 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
 204 {
 205
 206         float tr = matrix[0*4+0] + matrix[1*4+1] + matrix[2*4+2];
 207
 208         // check the diagonal
 209
 210         if (tr > 0.0f )
 211         {
 212                 float s = (float) sqrt ( (double) (tr + 1.0f) );
 213                 quat[3] = s * 0.5f;
 214                 s = 0.5f / s;
 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;
 218
 219         }
 220         else
 221         {
 222                 // diagonal is negative
 223                 int nxt[3] = {1, 2, 0};
 224                 float  qa[4];
 225
 226                 int i = 0;
 227
 228                 if (matrix[1*4+1] > matrix[0*4+0]) i = 1;
 229                 if (matrix[2*4+2] > matrix[i*4+i]) i = 2;
 230
 231                 int j = nxt[i];
 232                 int k = nxt[j];
 233
 234                 float s = sqrtf ( ((matrix[i*4+i] - (matrix[j*4+j] + matrix[k*4+k])) + 1.0f) );
 235
 236                 qa[i] = s * 0.5f;
 237
 238                 if (s != 0.0f ) s = 0.5f / s;
 239
 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;
 243
 244                 quat[0] = qa[0];
 245                 quat[1] = qa[1];
 246                 quat[2] = qa[2];
 247                 quat[3] = qa[3];
 248         }
 249
 250
 251 }
 252
 253
 254 float fm_sphereVolume(float radius) // return's the volume of a sphere of this radius (4/3 PI * R cubed )
 255 {
 256         return (4.0f / 3.0f ) * FM_PI * radius * radius * radius;
 257 }