Extras/vectormathlibrary/include/vectormath/scalar/c/quat_aos.h

   1 /*
   2    Copyright (C) 2006, 2007 Sony Computer Entertainment Inc.
   3    All rights reserved.
   4
   5    Redistribution and use in source and binary forms,
   6    with or without modification, are permitted provided that the
   7    following conditions are met:
   8     * Redistributions of source code must retain the above copyright
   9       notice, this list of conditions and the following disclaimer.
  10     * Redistributions in binary form must reproduce the above copyright
  11       notice, this list of conditions and the following disclaimer in the
  12       documentation and/or other materials provided with the distribution.
  13     * Neither the name of the Sony Computer Entertainment Inc nor the names
  14       of its contributors may be used to endorse or promote products derived
  15       from this software without specific prior written permission.
  16
  17    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  18    AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  19    IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  20    ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
  21    LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
  22    CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
  23    SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
  24    INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  25    CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
  26    ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  27    POSSIBILITY OF SUCH DAMAGE.
  28 */
  29
  30 #ifndef _VECTORMATH_QUAT_AOS_C_H
  31 #define _VECTORMATH_QUAT_AOS_C_H
  32 #ifdef __cplusplus
  33 extern "C" {
  34 #endif /* __cplusplus */
  35
  36 /*-----------------------------------------------------------------------------
  37  * Definitions
  38  */
  39 #ifndef _VECTORMATH_INTERNAL_FUNCTIONS
  40 #define _VECTORMATH_INTERNAL_FUNCTIONS
  41
  42 #endif
  43
  44 static inline void vmathQCopy( VmathQuat *result, const VmathQuat *quat )
  45 {
  46     result->x = quat->x;
  47     result->y = quat->y;
  48     result->z = quat->z;
  49     result->w = quat->w;
  50 }
  51
  52 static inline void vmathQMakeFromElems( VmathQuat *result, float _x, float _y, float _z, float _w )
  53 {
  54     result->x = _x;
  55     result->y = _y;
  56     result->z = _z;
  57     result->w = _w;
  58 }
  59
  60 static inline void vmathQMakeFromV3Scalar( VmathQuat *result, const VmathVector3 *xyz, float _w )
  61 {
  62     vmathQSetXYZ( result, xyz );
  63     vmathQSetW( result, _w );
  64 }
  65
  66 static inline void vmathQMakeFromV4( VmathQuat *result, const VmathVector4 *vec )
  67 {
  68     result->x = vec->x;
  69     result->y = vec->y;
  70     result->z = vec->z;
  71     result->w = vec->w;
  72 }
  73
  74 static inline void vmathQMakeFromScalar( VmathQuat *result, float scalar )
  75 {
  76     result->x = scalar;
  77     result->y = scalar;
  78     result->z = scalar;
  79     result->w = scalar;
  80 }
  81
  82 static inline void vmathQMakeIdentity( VmathQuat *result )
  83 {
  84     vmathQMakeFromElems( result, 0.0f, 0.0f, 0.0f, 1.0f );
  85 }
  86
  87 static inline void vmathQLerp( VmathQuat *result, float t, const VmathQuat *quat0, const VmathQuat *quat1 )
  88 {
  89     VmathQuat tmpQ_0, tmpQ_1;
  90     vmathQSub( &tmpQ_0, quat1, quat0 );
  91     vmathQScalarMul( &tmpQ_1, &tmpQ_0, t );
  92     vmathQAdd( result, quat0, &tmpQ_1 );
  93 }
  94
  95 static inline void vmathQSlerp( VmathQuat *result, float t, const VmathQuat *unitQuat0, const VmathQuat *unitQuat1 )
  96 {
  97     VmathQuat start, tmpQ_0, tmpQ_1;
  98     float recipSinAngle, scale0, scale1, cosAngle, angle;
  99     cosAngle = vmathQDot( unitQuat0, unitQuat1 );
 100     if ( cosAngle < 0.0f ) {
 101         cosAngle = -cosAngle;
 102         vmathQNeg( &start, unitQuat0 );
 103     } else {
 104         vmathQCopy( &start, unitQuat0 );
 105     }
 106     if ( cosAngle < _VECTORMATH_SLERP_TOL ) {
 107         angle = acosf( cosAngle );
 108         recipSinAngle = ( 1.0f / sinf( angle ) );
 109         scale0 = ( sinf( ( ( 1.0f - t ) * angle ) ) * recipSinAngle );
 110         scale1 = ( sinf( ( t * angle ) ) * recipSinAngle );
 111     } else {
 112         scale0 = ( 1.0f - t );
 113         scale1 = t;
 114     }
 115     vmathQScalarMul( &tmpQ_0, &start, scale0 );
 116     vmathQScalarMul( &tmpQ_1, unitQuat1, scale1 );
 117     vmathQAdd( result, &tmpQ_0, &tmpQ_1 );
 118 }
 119
 120 static inline void vmathQSquad( VmathQuat *result, float t, const VmathQuat *unitQuat0, const VmathQuat *unitQuat1, const VmathQuat *unitQuat2, const VmathQuat *unitQuat3 )
 121 {
 122     VmathQuat tmp0, tmp1;
 123     vmathQSlerp( &tmp0, t, unitQuat0, unitQuat3 );
 124     vmathQSlerp( &tmp1, t, unitQuat1, unitQuat2 );
 125     vmathQSlerp( result, ( ( 2.0f * t ) * ( 1.0f - t ) ), &tmp0, &tmp1 );
 126 }
 127
 128 static inline void vmathQSetXYZ( VmathQuat *result, const VmathVector3 *vec )
 129 {
 130     result->x = vec->x;
 131     result->y = vec->y;
 132     result->z = vec->z;
 133 }
 134
 135 static inline void vmathQGetXYZ( VmathVector3 *result, const VmathQuat *quat )
 136 {
 137     vmathV3MakeFromElems( result, quat->x, quat->y, quat->z );
 138 }
 139
 140 static inline void vmathQSetX( VmathQuat *result, float _x )
 141 {
 142     result->x = _x;
 143 }
 144
 145 static inline float vmathQGetX( const VmathQuat *quat )
 146 {
 147     return quat->x;
 148 }
 149
 150 static inline void vmathQSetY( VmathQuat *result, float _y )
 151 {
 152     result->y = _y;
 153 }
 154
 155 static inline float vmathQGetY( const VmathQuat *quat )
 156 {
 157     return quat->y;
 158 }
 159
 160 static inline void vmathQSetZ( VmathQuat *result, float _z )
 161 {
 162     result->z = _z;
 163 }
 164
 165 static inline float vmathQGetZ( const VmathQuat *quat )
 166 {
 167     return quat->z;
 168 }
 169
 170 static inline void vmathQSetW( VmathQuat *result, float _w )
 171 {
 172     result->w = _w;
 173 }
 174
 175 static inline float vmathQGetW( const VmathQuat *quat )
 176 {
 177     return quat->w;
 178 }
 179
 180 static inline void vmathQSetElem( VmathQuat *result, int idx, float value )
 181 {
 182     *(&result->x + idx) = value;
 183 }
 184
 185 static inline float vmathQGetElem( const VmathQuat *quat, int idx )
 186 {
 187     return *(&quat->x + idx);
 188 }
 189
 190 static inline void vmathQAdd( VmathQuat *result, const VmathQuat *quat0, const VmathQuat *quat1 )
 191 {
 192     result->x = ( quat0->x + quat1->x );
 193     result->y = ( quat0->y + quat1->y );
 194     result->z = ( quat0->z + quat1->z );
 195     result->w = ( quat0->w + quat1->w );
 196 }
 197
 198 static inline void vmathQSub( VmathQuat *result, const VmathQuat *quat0, const VmathQuat *quat1 )
 199 {
 200     result->x = ( quat0->x - quat1->x );
 201     result->y = ( quat0->y - quat1->y );
 202     result->z = ( quat0->z - quat1->z );
 203     result->w = ( quat0->w - quat1->w );
 204 }
 205
 206 static inline void vmathQScalarMul( VmathQuat *result, const VmathQuat *quat, float scalar )
 207 {
 208     result->x = ( quat->x * scalar );
 209     result->y = ( quat->y * scalar );
 210     result->z = ( quat->z * scalar );
 211     result->w = ( quat->w * scalar );
 212 }
 213
 214 static inline void vmathQScalarDiv( VmathQuat *result, const VmathQuat *quat, float scalar )
 215 {
 216     result->x = ( quat->x / scalar );
 217     result->y = ( quat->y / scalar );
 218     result->z = ( quat->z / scalar );
 219     result->w = ( quat->w / scalar );
 220 }
 221
 222 static inline void vmathQNeg( VmathQuat *result, const VmathQuat *quat )
 223 {
 224     result->x = -quat->x;
 225     result->y = -quat->y;
 226     result->z = -quat->z;
 227     result->w = -quat->w;
 228 }
 229
 230 static inline float vmathQDot( const VmathQuat *quat0, const VmathQuat *quat1 )
 231 {
 232     float result;
 233     result = ( quat0->x * quat1->x );
 234     result = ( result + ( quat0->y * quat1->y ) );
 235     result = ( result + ( quat0->z * quat1->z ) );
 236     result = ( result + ( quat0->w * quat1->w ) );
 237     return result;
 238 }
 239
 240 static inline float vmathQNorm( const VmathQuat *quat )
 241 {
 242     float result;
 243     result = ( quat->x * quat->x );
 244     result = ( result + ( quat->y * quat->y ) );
 245     result = ( result + ( quat->z * quat->z ) );
 246     result = ( result + ( quat->w * quat->w ) );
 247     return result;
 248 }
 249
 250 static inline float vmathQLength( const VmathQuat *quat )
 251 {
 252     return sqrtf( vmathQNorm( quat ) );
 253 }
 254
 255 static inline void vmathQNormalize( VmathQuat *result, const VmathQuat *quat )
 256 {
 257     float lenSqr, lenInv;
 258     lenSqr = vmathQNorm( quat );
 259     lenInv = ( 1.0f / sqrtf( lenSqr ) );
 260     result->x = ( quat->x * lenInv );
 261     result->y = ( quat->y * lenInv );
 262     result->z = ( quat->z * lenInv );
 263     result->w = ( quat->w * lenInv );
 264 }
 265
 266 static inline void vmathQMakeRotationArc( VmathQuat *result, const VmathVector3 *unitVec0, const VmathVector3 *unitVec1 )
 267 {
 268     VmathVector3 tmpV3_0, tmpV3_1;
 269     float cosHalfAngleX2, recipCosHalfAngleX2;
 270     cosHalfAngleX2 = sqrtf( ( 2.0f * ( 1.0f + vmathV3Dot( unitVec0, unitVec1 ) ) ) );
 271     recipCosHalfAngleX2 = ( 1.0f / cosHalfAngleX2 );
 272     vmathV3Cross( &tmpV3_0, unitVec0, unitVec1 );
 273     vmathV3ScalarMul( &tmpV3_1, &tmpV3_0, recipCosHalfAngleX2 );
 274     vmathQMakeFromV3Scalar( result, &tmpV3_1, ( cosHalfAngleX2 * 0.5f ) );
 275 }
 276
 277 static inline void vmathQMakeRotationAxis( VmathQuat *result, float radians, const VmathVector3 *unitVec )
 278 {
 279     VmathVector3 tmpV3_0;
 280     float s, c, angle;
 281     angle = ( radians * 0.5f );
 282     s = sinf( angle );
 283     c = cosf( angle );
 284     vmathV3ScalarMul( &tmpV3_0, unitVec, s );
 285     vmathQMakeFromV3Scalar( result, &tmpV3_0, c );
 286 }
 287
 288 static inline void vmathQMakeRotationX( VmathQuat *result, float radians )
 289 {
 290     float s, c, angle;
 291     angle = ( radians * 0.5f );
 292     s = sinf( angle );
 293     c = cosf( angle );
 294     vmathQMakeFromElems( result, s, 0.0f, 0.0f, c );
 295 }
 296
 297 static inline void vmathQMakeRotationY( VmathQuat *result, float radians )
 298 {
 299     float s, c, angle;
 300     angle = ( radians * 0.5f );
 301     s = sinf( angle );
 302     c = cosf( angle );
 303     vmathQMakeFromElems( result, 0.0f, s, 0.0f, c );
 304 }
 305
 306 static inline void vmathQMakeRotationZ( VmathQuat *result, float radians )
 307 {
 308     float s, c, angle;
 309     angle = ( radians * 0.5f );
 310     s = sinf( angle );
 311     c = cosf( angle );
 312     vmathQMakeFromElems( result, 0.0f, 0.0f, s, c );
 313 }
 314
 315 static inline void vmathQMul( VmathQuat *result, const VmathQuat *quat0, const VmathQuat *quat1 )
 316 {
 317     float tmpX, tmpY, tmpZ, tmpW;
 318     tmpX = ( ( ( ( quat0->w * quat1->x ) + ( quat0->x * quat1->w ) ) + ( quat0->y * quat1->z ) ) - ( quat0->z * quat1->y ) );
 319     tmpY = ( ( ( ( quat0->w * quat1->y ) + ( quat0->y * quat1->w ) ) + ( quat0->z * quat1->x ) ) - ( quat0->x * quat1->z ) );
 320     tmpZ = ( ( ( ( quat0->w * quat1->z ) + ( quat0->z * quat1->w ) ) + ( quat0->x * quat1->y ) ) - ( quat0->y * quat1->x ) );
 321     tmpW = ( ( ( ( quat0->w * quat1->w ) - ( quat0->x * quat1->x ) ) - ( quat0->y * quat1->y ) ) - ( quat0->z * quat1->z ) );
 322     vmathQMakeFromElems( result, tmpX, tmpY, tmpZ, tmpW );
 323 }
 324
 325 static inline void vmathQRotate( VmathVector3 *result, const VmathQuat *quat, const VmathVector3 *vec )
 326 {
 327     float tmpX, tmpY, tmpZ, tmpW;
 328     tmpX = ( ( ( quat->w * vec->x ) + ( quat->y * vec->z ) ) - ( quat->z * vec->y ) );
 329     tmpY = ( ( ( quat->w * vec->y ) + ( quat->z * vec->x ) ) - ( quat->x * vec->z ) );
 330     tmpZ = ( ( ( quat->w * vec->z ) + ( quat->x * vec->y ) ) - ( quat->y * vec->x ) );
 331     tmpW = ( ( ( quat->x * vec->x ) + ( quat->y * vec->y ) ) + ( quat->z * vec->z ) );
 332     result->x = ( ( ( ( tmpW * quat->x ) + ( tmpX * quat->w ) ) - ( tmpY * quat->z ) ) + ( tmpZ * quat->y ) );
 333     result->y = ( ( ( ( tmpW * quat->y ) + ( tmpY * quat->w ) ) - ( tmpZ * quat->x ) ) + ( tmpX * quat->z ) );
 334     result->z = ( ( ( ( tmpW * quat->z ) + ( tmpZ * quat->w ) ) - ( tmpX * quat->y ) ) + ( tmpY * quat->x ) );
 335 }
 336
 337 static inline void vmathQConj( VmathQuat *result, const VmathQuat *quat )
 338 {
 339     vmathQMakeFromElems( result, -quat->x, -quat->y, -quat->z, quat->w );
 340 }
 341
 342 static inline void vmathQSelect( VmathQuat *result, const VmathQuat *quat0, const VmathQuat *quat1, unsigned int select1 )
 343 {
 344     result->x = ( select1 )? quat1->x : quat0->x;
 345     result->y = ( select1 )? quat1->y : quat0->y;
 346     result->z = ( select1 )? quat1->z : quat0->z;
 347     result->w = ( select1 )? quat1->w : quat0->w;
 348 }
 349
 350 #ifdef _VECTORMATH_DEBUG
 351
 352 static inline void vmathQPrint( const VmathQuat *quat )
 353 {
 354     printf( "( %f %f %f %f )\n", quat->x, quat->y, quat->z, quat->w );
 355 }
 356
 357 static inline void vmathQPrints( const VmathQuat *quat, const char *name )
 358 {
 359     printf( "%s: ( %f %f %f %f )\n", name, quat->x, quat->y, quat->z, quat->w );
 360 }
 361
 362 #endif
 363
 364 #ifdef __cplusplus
 365 }
 366 #endif /* __cplusplus */
 367
 368 #endif