Extras/vectormathlibrary/include/vectormath/ppu/c/vec_soa.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_VEC_SOA_C_H
  31 #define _VECTORMATH_VEC_SOA_C_H
  32 #ifdef __cplusplus
  33 extern "C" {
  34 #endif /* __cplusplus */
  35
  36 /*-----------------------------------------------------------------------------
  37  * Constants
  38  * for permutes, words are labeled [x,y,z,w] [a,b,c,d]
  39  */
  40 #define _VECTORMATH_PERM_X 0x00010203
  41 #define _VECTORMATH_PERM_Y 0x04050607
  42 #define _VECTORMATH_PERM_Z 0x08090a0b
  43 #define _VECTORMATH_PERM_W 0x0c0d0e0f
  44 #define _VECTORMATH_PERM_A 0x10111213
  45 #define _VECTORMATH_PERM_B 0x14151617
  46 #define _VECTORMATH_PERM_C 0x18191a1b
  47 #define _VECTORMATH_PERM_D 0x1c1d1e1f
  48 #define _VECTORMATH_PERM_ZBWX ((vec_uchar16)(vec_uint4){ _VECTORMATH_PERM_Z, _VECTORMATH_PERM_B, _VECTORMATH_PERM_W, _VECTORMATH_PERM_X })
  49 #define _VECTORMATH_PERM_XCYX ((vec_uchar16)(vec_uint4){ _VECTORMATH_PERM_X, _VECTORMATH_PERM_C, _VECTORMATH_PERM_Y, _VECTORMATH_PERM_X })
  50 #define _VECTORMATH_PERM_ZDWX ((vec_uchar16)(vec_uint4){ _VECTORMATH_PERM_Z, _VECTORMATH_PERM_D, _VECTORMATH_PERM_W, _VECTORMATH_PERM_X })
  51 #define _VECTORMATH_PERM_ZCXA ((vec_uchar16)(vec_uint4){ _VECTORMATH_PERM_Z, _VECTORMATH_PERM_C, _VECTORMATH_PERM_X, _VECTORMATH_PERM_A })
  52 #define _VECTORMATH_PERM_XBZD ((vec_uchar16)(vec_uint4){ _VECTORMATH_PERM_X, _VECTORMATH_PERM_B, _VECTORMATH_PERM_Z, _VECTORMATH_PERM_D })
  53 #define _VECTORMATH_PERM_WDYB ((vec_uchar16)(vec_uint4){ _VECTORMATH_PERM_W, _VECTORMATH_PERM_D, _VECTORMATH_PERM_Y, _VECTORMATH_PERM_B })
  54 #define _VECTORMATH_PERM_ZBXD ((vec_uchar16)(vec_uint4){ _VECTORMATH_PERM_Z, _VECTORMATH_PERM_B, _VECTORMATH_PERM_X, _VECTORMATH_PERM_D })
  55 #define _VECTORMATH_PERM_WCYA ((vec_uchar16)(vec_uint4){ _VECTORMATH_PERM_W, _VECTORMATH_PERM_C, _VECTORMATH_PERM_Y, _VECTORMATH_PERM_A })
  56 #define _VECTORMATH_PERM_XDZB ((vec_uchar16)(vec_uint4){ _VECTORMATH_PERM_X, _VECTORMATH_PERM_D, _VECTORMATH_PERM_Z, _VECTORMATH_PERM_B })
  57 #define _VECTORMATH_SLERP_TOL 0.999f
  58
  59 /*-----------------------------------------------------------------------------
  60  * Definitions
  61  */
  62 #ifndef _VECTORMATH_INTERNAL_FUNCTIONS
  63 #define _VECTORMATH_INTERNAL_FUNCTIONS
  64
  65 #endif
  66
  67 static inline void vmathSoaV3Copy( VmathSoaVector3 *result, const VmathSoaVector3 *vec )
  68 {
  69     result->x = vec->x;
  70     result->y = vec->y;
  71     result->z = vec->z;
  72 }
  73
  74 static inline void vmathSoaV3MakeFromElems( VmathSoaVector3 *result, vec_float4 _x, vec_float4 _y, vec_float4 _z )
  75 {
  76     result->x = _x;
  77     result->y = _y;
  78     result->z = _z;
  79 }
  80
  81 static inline void vmathSoaV3MakeFromP3( VmathSoaVector3 *result, const VmathSoaPoint3 *pnt )
  82 {
  83     result->x = pnt->x;
  84     result->y = pnt->y;
  85     result->z = pnt->z;
  86 }
  87
  88 static inline void vmathSoaV3MakeFromScalar( VmathSoaVector3 *result, vec_float4 scalar )
  89 {
  90     result->x = scalar;
  91     result->y = scalar;
  92     result->z = scalar;
  93 }
  94
  95 static inline void vmathSoaV3MakeFromAos( VmathSoaVector3 *result, const VmathVector3 *vec )
  96 {
  97     vec_float4 vec128 = vec->vec128;
  98     result->x = vec_splat( vec128, 0 );
  99     result->y = vec_splat( vec128, 1 );
 100     result->z = vec_splat( vec128, 2 );
 101 }
 102
 103 static inline void vmathSoaV3MakeFrom4Aos( VmathSoaVector3 *result, const VmathVector3 *vec0, const VmathVector3 *vec1, const VmathVector3 *vec2, const VmathVector3 *vec3 )
 104 {
 105     vec_float4 tmp0, tmp1, tmp2, tmp3;
 106     tmp0 = vec_mergeh( vec0->vec128, vec2->vec128 );
 107     tmp1 = vec_mergeh( vec1->vec128, vec3->vec128 );
 108     tmp2 = vec_mergel( vec0->vec128, vec2->vec128 );
 109     tmp3 = vec_mergel( vec1->vec128, vec3->vec128 );
 110     result->x = vec_mergeh( tmp0, tmp1 );
 111     result->y = vec_mergel( tmp0, tmp1 );
 112     result->z = vec_mergeh( tmp2, tmp3 );
 113 }
 114
 115 static inline void vmathSoaV3MakeXAxis( VmathSoaVector3 *result )
 116 {
 117     vmathSoaV3MakeFromElems( result, ((vec_float4){1.0f,1.0f,1.0f,1.0f}), ((vec_float4){0.0f,0.0f,0.0f,0.0f}), ((vec_float4){0.0f,0.0f,0.0f,0.0f}) );
 118 }
 119
 120 static inline void vmathSoaV3MakeYAxis( VmathSoaVector3 *result )
 121 {
 122     vmathSoaV3MakeFromElems( result, ((vec_float4){0.0f,0.0f,0.0f,0.0f}), ((vec_float4){1.0f,1.0f,1.0f,1.0f}), ((vec_float4){0.0f,0.0f,0.0f,0.0f}) );
 123 }
 124
 125 static inline void vmathSoaV3MakeZAxis( VmathSoaVector3 *result )
 126 {
 127     vmathSoaV3MakeFromElems( result, ((vec_float4){0.0f,0.0f,0.0f,0.0f}), ((vec_float4){0.0f,0.0f,0.0f,0.0f}), ((vec_float4){1.0f,1.0f,1.0f,1.0f}) );
 128 }
 129
 130 static inline void vmathSoaV3Lerp( VmathSoaVector3 *result, vec_float4 t, const VmathSoaVector3 *vec0, const VmathSoaVector3 *vec1 )
 131 {
 132     VmathSoaVector3 tmpV3_0, tmpV3_1;
 133     vmathSoaV3Sub( &tmpV3_0, vec1, vec0 );
 134     vmathSoaV3ScalarMul( &tmpV3_1, &tmpV3_0, t );
 135     vmathSoaV3Add( result, vec0, &tmpV3_1 );
 136 }
 137
 138 static inline void vmathSoaV3Slerp( VmathSoaVector3 *result, vec_float4 t, const VmathSoaVector3 *unitVec0, const VmathSoaVector3 *unitVec1 )
 139 {
 140     VmathSoaVector3 tmpV3_0, tmpV3_1;
 141     vec_float4 recipSinAngle, scale0, scale1, cosAngle, angle;
 142     vec_uint4 selectMask;
 143     cosAngle = vmathSoaV3Dot( unitVec0, unitVec1 );
 144     selectMask = (vec_uint4)vec_cmpgt( (vec_float4){_VECTORMATH_SLERP_TOL,_VECTORMATH_SLERP_TOL,_VECTORMATH_SLERP_TOL,_VECTORMATH_SLERP_TOL}, cosAngle );
 145     angle = acosf4( cosAngle );
 146     recipSinAngle = divf4( ((vec_float4){1.0f,1.0f,1.0f,1.0f}), sinf4( angle ) );
 147     scale0 = vec_sel( vec_sub( ((vec_float4){1.0f,1.0f,1.0f,1.0f}), t ), vec_madd( sinf4( vec_madd( vec_sub( ((vec_float4){1.0f,1.0f,1.0f,1.0f}), t ), angle, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) ) ), recipSinAngle, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) ), selectMask );
 148     scale1 = vec_sel( t, vec_madd( sinf4( vec_madd( t, angle, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) ) ), recipSinAngle, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) ), selectMask );
 149     vmathSoaV3ScalarMul( &tmpV3_0, unitVec0, scale0 );
 150     vmathSoaV3ScalarMul( &tmpV3_1, unitVec1, scale1 );
 151     vmathSoaV3Add( result, &tmpV3_0, &tmpV3_1 );
 152 }
 153
 154 static inline void vmathSoaV3Get4Aos( const VmathSoaVector3 *vec, VmathVector3 *result0, VmathVector3 *result1, VmathVector3 *result2, VmathVector3 *result3 )
 155 {
 156     vec_float4 tmp0, tmp1;
 157     tmp0 = vec_mergeh( vec->x, vec->z );
 158     tmp1 = vec_mergel( vec->x, vec->z );
 159     vmathV3MakeFrom128( result0, vec_mergeh( tmp0, vec->y ) );
 160     vmathV3MakeFrom128( result1, vec_perm( tmp0, vec->y, _VECTORMATH_PERM_ZBWX ) );
 161     vmathV3MakeFrom128( result2, vec_perm( tmp1, vec->y, _VECTORMATH_PERM_XCYX ) );
 162     vmathV3MakeFrom128( result3, vec_perm( tmp1, vec->y, _VECTORMATH_PERM_ZDWX ) );
 163 }
 164
 165 static inline void vmathSoaV3LoadXYZArray( VmathSoaVector3 *vec, const vec_float4 *threeQuads )
 166 {
 167     vec_float4 xyxy, yzyz, zxzx, xyzx, yzxy, zxyz;
 168     xyzx = threeQuads[0];
 169     yzxy = threeQuads[1];
 170     zxyz = threeQuads[2];
 171     xyxy = vec_sld( yzxy, xyzx, 8 );
 172     zxzx = vec_sld( xyzx, zxyz, 8 );
 173     yzyz = vec_sld( zxyz, yzxy, 8 );
 174     vmathSoaV3SetX( vec, vec_perm( xyxy, zxzx, _VECTORMATH_PERM_ZBXD ) );
 175     vmathSoaV3SetY( vec, vec_perm( xyxy, yzyz, _VECTORMATH_PERM_WCYA ) );
 176     vmathSoaV3SetZ( vec, vec_perm( zxzx, yzyz, _VECTORMATH_PERM_XDZB ) );
 177 }
 178
 179 static inline void vmathSoaV3StoreXYZArray( const VmathSoaVector3 *vec, vec_float4 *threeQuads )
 180 {
 181     vec_float4 xyzx, yzxy, zxyz, xyxy, zxzx, yzyz;
 182     xyxy = vec_perm( vec->x, vec->y, _VECTORMATH_PERM_ZCXA );
 183     zxzx = vec_perm( vec->z, vec->x, _VECTORMATH_PERM_XBZD );
 184     yzyz = vec_perm( vec->y, vec->z, _VECTORMATH_PERM_WDYB );
 185     xyzx = vec_sld( xyxy, zxzx, 8 );
 186     yzxy = vec_sld( yzyz, xyxy, 8 );
 187     zxyz = vec_sld( zxzx, yzyz, 8 );
 188     threeQuads[0] = xyzx;
 189     threeQuads[1] = yzxy;
 190     threeQuads[2] = zxyz;
 191 }
 192
 193 static inline void vmathSoaV3StoreHalfFloats( const VmathSoaVector3 *vec0, const VmathSoaVector3 *vec1, vec_ushort8 *threeQuads )
 194 {
 195     vec_float4 xyz0[3];
 196     vec_float4 xyz1[3];
 197     vmathSoaV3StoreXYZArray( vec0, xyz0 );
 198     vmathSoaV3StoreXYZArray( vec1, xyz1 );
 199     threeQuads[0] = _vmath2VfToHalfFloats(xyz0[0], xyz0[1]);
 200     threeQuads[1] = _vmath2VfToHalfFloats(xyz0[2], xyz1[0]);
 201     threeQuads[2] = _vmath2VfToHalfFloats(xyz1[1], xyz1[2]);
 202 }
 203
 204 static inline void vmathSoaV3SetX( VmathSoaVector3 *result, vec_float4 _x )
 205 {
 206     result->x = _x;
 207 }
 208
 209 static inline vec_float4 vmathSoaV3GetX( const VmathSoaVector3 *vec )
 210 {
 211     return vec->x;
 212 }
 213
 214 static inline void vmathSoaV3SetY( VmathSoaVector3 *result, vec_float4 _y )
 215 {
 216     result->y = _y;
 217 }
 218
 219 static inline vec_float4 vmathSoaV3GetY( const VmathSoaVector3 *vec )
 220 {
 221     return vec->y;
 222 }
 223
 224 static inline void vmathSoaV3SetZ( VmathSoaVector3 *result, vec_float4 _z )
 225 {
 226     result->z = _z;
 227 }
 228
 229 static inline vec_float4 vmathSoaV3GetZ( const VmathSoaVector3 *vec )
 230 {
 231     return vec->z;
 232 }
 233
 234 static inline void vmathSoaV3SetElem( VmathSoaVector3 *result, int idx, vec_float4 value )
 235 {
 236     *(&result->x + idx) = value;
 237 }
 238
 239 static inline vec_float4 vmathSoaV3GetElem( const VmathSoaVector3 *vec, int idx )
 240 {
 241     return *(&vec->x + idx);
 242 }
 243
 244 static inline void vmathSoaV3Add( VmathSoaVector3 *result, const VmathSoaVector3 *vec0, const VmathSoaVector3 *vec1 )
 245 {
 246     result->x = vec_add( vec0->x, vec1->x );
 247     result->y = vec_add( vec0->y, vec1->y );
 248     result->z = vec_add( vec0->z, vec1->z );
 249 }
 250
 251 static inline void vmathSoaV3Sub( VmathSoaVector3 *result, const VmathSoaVector3 *vec0, const VmathSoaVector3 *vec1 )
 252 {
 253     result->x = vec_sub( vec0->x, vec1->x );
 254     result->y = vec_sub( vec0->y, vec1->y );
 255     result->z = vec_sub( vec0->z, vec1->z );
 256 }
 257
 258 static inline void vmathSoaV3AddP3( VmathSoaPoint3 *result, const VmathSoaVector3 *vec, const VmathSoaPoint3 *pnt1 )
 259 {
 260     result->x = vec_add( vec->x, pnt1->x );
 261     result->y = vec_add( vec->y, pnt1->y );
 262     result->z = vec_add( vec->z, pnt1->z );
 263 }
 264
 265 static inline void vmathSoaV3ScalarMul( VmathSoaVector3 *result, const VmathSoaVector3 *vec, vec_float4 scalar )
 266 {
 267     result->x = vec_madd( vec->x, scalar, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) );
 268     result->y = vec_madd( vec->y, scalar, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) );
 269     result->z = vec_madd( vec->z, scalar, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) );
 270 }
 271
 272 static inline void vmathSoaV3ScalarDiv( VmathSoaVector3 *result, const VmathSoaVector3 *vec, vec_float4 scalar )
 273 {
 274     result->x = divf4( vec->x, scalar );
 275     result->y = divf4( vec->y, scalar );
 276     result->z = divf4( vec->z, scalar );
 277 }
 278
 279 static inline void vmathSoaV3Neg( VmathSoaVector3 *result, const VmathSoaVector3 *vec )
 280 {
 281     result->x = negatef4( vec->x );
 282     result->y = negatef4( vec->y );
 283     result->z = negatef4( vec->z );
 284 }
 285
 286 static inline void vmathSoaV3MulPerElem( VmathSoaVector3 *result, const VmathSoaVector3 *vec0, const VmathSoaVector3 *vec1 )
 287 {
 288     result->x = vec_madd( vec0->x, vec1->x, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) );
 289     result->y = vec_madd( vec0->y, vec1->y, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) );
 290     result->z = vec_madd( vec0->z, vec1->z, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) );
 291 }
 292
 293 static inline void vmathSoaV3DivPerElem( VmathSoaVector3 *result, const VmathSoaVector3 *vec0, const VmathSoaVector3 *vec1 )
 294 {
 295     result->x = divf4( vec0->x, vec1->x );
 296     result->y = divf4( vec0->y, vec1->y );
 297     result->z = divf4( vec0->z, vec1->z );
 298 }
 299
 300 static inline void vmathSoaV3RecipPerElem( VmathSoaVector3 *result, const VmathSoaVector3 *vec )
 301 {
 302     result->x = divf4( ((vec_float4){1.0f,1.0f,1.0f,1.0f}), vec->x );
 303     result->y = divf4( ((vec_float4){1.0f,1.0f,1.0f,1.0f}), vec->y );
 304     result->z = divf4( ((vec_float4){1.0f,1.0f,1.0f,1.0f}), vec->z );
 305 }
 306
 307 static inline void vmathSoaV3SqrtPerElem( VmathSoaVector3 *result, const VmathSoaVector3 *vec )
 308 {
 309     result->x = sqrtf4( vec->x );
 310     result->y = sqrtf4( vec->y );
 311     result->z = sqrtf4( vec->z );
 312 }
 313
 314 static inline void vmathSoaV3RsqrtPerElem( VmathSoaVector3 *result, const VmathSoaVector3 *vec )
 315 {
 316     result->x = divf4( ((vec_float4){1.0f,1.0f,1.0f,1.0f}), sqrtf4( vec->x ) );
 317     result->y = divf4( ((vec_float4){1.0f,1.0f,1.0f,1.0f}), sqrtf4( vec->y ) );
 318     result->z = divf4( ((vec_float4){1.0f,1.0f,1.0f,1.0f}), sqrtf4( vec->z ) );
 319 }
 320
 321 static inline void vmathSoaV3AbsPerElem( VmathSoaVector3 *result, const VmathSoaVector3 *vec )
 322 {
 323     result->x = fabsf4( vec->x );
 324     result->y = fabsf4( vec->y );
 325     result->z = fabsf4( vec->z );
 326 }
 327
 328 static inline void vmathSoaV3CopySignPerElem( VmathSoaVector3 *result, const VmathSoaVector3 *vec0, const VmathSoaVector3 *vec1 )
 329 {
 330     result->x = copysignf4( vec0->x, vec1->x );
 331     result->y = copysignf4( vec0->y, vec1->y );
 332     result->z = copysignf4( vec0->z, vec1->z );
 333 }
 334
 335 static inline void vmathSoaV3MaxPerElem( VmathSoaVector3 *result, const VmathSoaVector3 *vec0, const VmathSoaVector3 *vec1 )
 336 {
 337     result->x = fmaxf4( vec0->x, vec1->x );
 338     result->y = fmaxf4( vec0->y, vec1->y );
 339     result->z = fmaxf4( vec0->z, vec1->z );
 340 }
 341
 342 static inline vec_float4 vmathSoaV3MaxElem( const VmathSoaVector3 *vec )
 343 {
 344     vec_float4 result;
 345     result = fmaxf4( vec->x, vec->y );
 346     result = fmaxf4( vec->z, result );
 347     return result;
 348 }
 349
 350 static inline void vmathSoaV3MinPerElem( VmathSoaVector3 *result, const VmathSoaVector3 *vec0, const VmathSoaVector3 *vec1 )
 351 {
 352     result->x = fminf4( vec0->x, vec1->x );
 353     result->y = fminf4( vec0->y, vec1->y );
 354     result->z = fminf4( vec0->z, vec1->z );
 355 }
 356
 357 static inline vec_float4 vmathSoaV3MinElem( const VmathSoaVector3 *vec )
 358 {
 359     vec_float4 result;
 360     result = fminf4( vec->x, vec->y );
 361     result = fminf4( vec->z, result );
 362     return result;
 363 }
 364
 365 static inline vec_float4 vmathSoaV3Sum( const VmathSoaVector3 *vec )
 366 {
 367     vec_float4 result;
 368     result = vec_add( vec->x, vec->y );
 369     result = vec_add( result, vec->z );
 370     return result;
 371 }
 372
 373 static inline vec_float4 vmathSoaV3Dot( const VmathSoaVector3 *vec0, const VmathSoaVector3 *vec1 )
 374 {
 375     vec_float4 result;
 376     result = vec_madd( vec0->x, vec1->x, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) );
 377     result = vec_add( result, vec_madd( vec0->y, vec1->y, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) ) );
 378     result = vec_add( result, vec_madd( vec0->z, vec1->z, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) ) );
 379     return result;
 380 }
 381
 382 static inline vec_float4 vmathSoaV3LengthSqr( const VmathSoaVector3 *vec )
 383 {
 384     vec_float4 result;
 385     result = vec_madd( vec->x, vec->x, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) );
 386     result = vec_add( result, vec_madd( vec->y, vec->y, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) ) );
 387     result = vec_add( result, vec_madd( vec->z, vec->z, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) ) );
 388     return result;
 389 }
 390
 391 static inline vec_float4 vmathSoaV3Length( const VmathSoaVector3 *vec )
 392 {
 393     return sqrtf4( vmathSoaV3LengthSqr( vec ) );
 394 }
 395
 396 static inline void vmathSoaV3Normalize( VmathSoaVector3 *result, const VmathSoaVector3 *vec )
 397 {
 398     vec_float4 lenSqr, lenInv;
 399     lenSqr = vmathSoaV3LengthSqr( vec );
 400     lenInv = divf4( ((vec_float4){1.0f,1.0f,1.0f,1.0f}), sqrtf4( lenSqr ) );
 401     result->x = vec_madd( vec->x, lenInv, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) );
 402     result->y = vec_madd( vec->y, lenInv, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) );
 403     result->z = vec_madd( vec->z, lenInv, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) );
 404 }
 405
 406 static inline void vmathSoaV3Cross( VmathSoaVector3 *result, const VmathSoaVector3 *vec0, const VmathSoaVector3 *vec1 )
 407 {
 408     vec_float4 tmpX, tmpY, tmpZ;
 409     tmpX = vec_sub( vec_madd( vec0->y, vec1->z, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) ), vec_madd( vec0->z, vec1->y, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) ) );
 410     tmpY = vec_sub( vec_madd( vec0->z, vec1->x, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) ), vec_madd( vec0->x, vec1->z, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) ) );
 411     tmpZ = vec_sub( vec_madd( vec0->x, vec1->y, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) ), vec_madd( vec0->y, vec1->x, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) ) );
 412     vmathSoaV3MakeFromElems( result, tmpX, tmpY, tmpZ );
 413 }
 414
 415 static inline void vmathSoaV3Select( VmathSoaVector3 *result, const VmathSoaVector3 *vec0, const VmathSoaVector3 *vec1, vec_uint4 select1 )
 416 {
 417     result->x = vec_sel( vec0->x, vec1->x, select1 );
 418     result->y = vec_sel( vec0->y, vec1->y, select1 );
 419     result->z = vec_sel( vec0->z, vec1->z, select1 );
 420 }
 421
 422 #ifdef _VECTORMATH_DEBUG
 423
 424 static inline void vmathSoaV3Print( const VmathSoaVector3 *vec )
 425 {
 426     VmathVector3 vec0, vec1, vec2, vec3;
 427     vmathSoaV3Get4Aos( vec, &vec0, &vec1, &vec2, &vec3 );
 428     printf("slot 0:\n");
 429     vmathV3Print( &vec0 );
 430     printf("slot 1:\n");
 431     vmathV3Print( &vec1 );
 432     printf("slot 2:\n");
 433     vmathV3Print( &vec2 );
 434     printf("slot 3:\n");
 435     vmathV3Print( &vec3 );
 436 }
 437
 438 static inline void vmathSoaV3Prints( const VmathSoaVector3 *vec, const char *name )
 439 {
 440     VmathVector3 vec0, vec1, vec2, vec3;
 441     printf( "%s:\n", name );
 442     vmathSoaV3Get4Aos( vec, &vec0, &vec1, &vec2, &vec3 );
 443     printf("slot 0:\n");
 444     vmathV3Print( &vec0 );
 445     printf("slot 1:\n");
 446     vmathV3Print( &vec1 );
 447     printf("slot 2:\n");
 448     vmathV3Print( &vec2 );
 449     printf("slot 3:\n");
 450     vmathV3Print( &vec3 );
 451 }
 452
 453 #endif
 454
 455 static inline void vmathSoaV4Copy( VmathSoaVector4 *result, const VmathSoaVector4 *vec )
 456 {
 457     result->x = vec->x;
 458     result->y = vec->y;
 459     result->z = vec->z;
 460     result->w = vec->w;
 461 }
 462
 463 static inline void vmathSoaV4MakeFromElems( VmathSoaVector4 *result, vec_float4 _x, vec_float4 _y, vec_float4 _z, vec_float4 _w )
 464 {
 465     result->x = _x;
 466     result->y = _y;
 467     result->z = _z;
 468     result->w = _w;
 469 }
 470
 471 static inline void vmathSoaV4MakeFromV3Scalar( VmathSoaVector4 *result, const VmathSoaVector3 *xyz, vec_float4 _w )
 472 {
 473     vmathSoaV4SetXYZ( result, xyz );
 474     vmathSoaV4SetW( result, _w );
 475 }
 476
 477 static inline void vmathSoaV4MakeFromV3( VmathSoaVector4 *result, const VmathSoaVector3 *vec )
 478 {
 479     result->x = vec->x;
 480     result->y = vec->y;
 481     result->z = vec->z;
 482     result->w = ((vec_float4){0.0f,0.0f,0.0f,0.0f});
 483 }
 484
 485 static inline void vmathSoaV4MakeFromP3( VmathSoaVector4 *result, const VmathSoaPoint3 *pnt )
 486 {
 487     result->x = pnt->x;
 488     result->y = pnt->y;
 489     result->z = pnt->z;
 490     result->w = ((vec_float4){1.0f,1.0f,1.0f,1.0f});
 491 }
 492
 493 static inline void vmathSoaV4MakeFromQ( VmathSoaVector4 *result, const VmathSoaQuat *quat )
 494 {
 495     result->x = quat->x;
 496     result->y = quat->y;
 497     result->z = quat->z;
 498     result->w = quat->w;
 499 }
 500
 501 static inline void vmathSoaV4MakeFromScalar( VmathSoaVector4 *result, vec_float4 scalar )
 502 {
 503     result->x = scalar;
 504     result->y = scalar;
 505     result->z = scalar;
 506     result->w = scalar;
 507 }
 508
 509 static inline void vmathSoaV4MakeFromAos( VmathSoaVector4 *result, const VmathVector4 *vec )
 510 {
 511     vec_float4 vec128 = vec->vec128;
 512     result->x = vec_splat( vec128, 0 );
 513     result->y = vec_splat( vec128, 1 );
 514     result->z = vec_splat( vec128, 2 );
 515     result->w = vec_splat( vec128, 3 );
 516 }
 517
 518 static inline void vmathSoaV4MakeFrom4Aos( VmathSoaVector4 *result, const VmathVector4 *vec0, const VmathVector4 *vec1, const VmathVector4 *vec2, const VmathVector4 *vec3 )
 519 {
 520     vec_float4 tmp0, tmp1, tmp2, tmp3;
 521     tmp0 = vec_mergeh( vec0->vec128, vec2->vec128 );
 522     tmp1 = vec_mergeh( vec1->vec128, vec3->vec128 );
 523     tmp2 = vec_mergel( vec0->vec128, vec2->vec128 );
 524     tmp3 = vec_mergel( vec1->vec128, vec3->vec128 );
 525     result->x = vec_mergeh( tmp0, tmp1 );
 526     result->y = vec_mergel( tmp0, tmp1 );
 527     result->z = vec_mergeh( tmp2, tmp3 );
 528     result->w = vec_mergel( tmp2, tmp3 );
 529 }
 530
 531 static inline void vmathSoaV4MakeXAxis( VmathSoaVector4 *result )
 532 {
 533     vmathSoaV4MakeFromElems( result, ((vec_float4){1.0f,1.0f,1.0f,1.0f}), ((vec_float4){0.0f,0.0f,0.0f,0.0f}), ((vec_float4){0.0f,0.0f,0.0f,0.0f}), ((vec_float4){0.0f,0.0f,0.0f,0.0f}) );
 534 }
 535
 536 static inline void vmathSoaV4MakeYAxis( VmathSoaVector4 *result )
 537 {
 538     vmathSoaV4MakeFromElems( result, ((vec_float4){0.0f,0.0f,0.0f,0.0f}), ((vec_float4){1.0f,1.0f,1.0f,1.0f}), ((vec_float4){0.0f,0.0f,0.0f,0.0f}), ((vec_float4){0.0f,0.0f,0.0f,0.0f}) );
 539 }
 540
 541 static inline void vmathSoaV4MakeZAxis( VmathSoaVector4 *result )
 542 {
 543     vmathSoaV4MakeFromElems( result, ((vec_float4){0.0f,0.0f,0.0f,0.0f}), ((vec_float4){0.0f,0.0f,0.0f,0.0f}), ((vec_float4){1.0f,1.0f,1.0f,1.0f}), ((vec_float4){0.0f,0.0f,0.0f,0.0f}) );
 544 }
 545
 546 static inline void vmathSoaV4MakeWAxis( VmathSoaVector4 *result )
 547 {
 548     vmathSoaV4MakeFromElems( result, ((vec_float4){0.0f,0.0f,0.0f,0.0f}), ((vec_float4){0.0f,0.0f,0.0f,0.0f}), ((vec_float4){0.0f,0.0f,0.0f,0.0f}), ((vec_float4){1.0f,1.0f,1.0f,1.0f}) );
 549 }
 550
 551 static inline void vmathSoaV4Lerp( VmathSoaVector4 *result, vec_float4 t, const VmathSoaVector4 *vec0, const VmathSoaVector4 *vec1 )
 552 {
 553     VmathSoaVector4 tmpV4_0, tmpV4_1;
 554     vmathSoaV4Sub( &tmpV4_0, vec1, vec0 );
 555     vmathSoaV4ScalarMul( &tmpV4_1, &tmpV4_0, t );
 556     vmathSoaV4Add( result, vec0, &tmpV4_1 );
 557 }
 558
 559 static inline void vmathSoaV4Slerp( VmathSoaVector4 *result, vec_float4 t, const VmathSoaVector4 *unitVec0, const VmathSoaVector4 *unitVec1 )
 560 {
 561     VmathSoaVector4 tmpV4_0, tmpV4_1;
 562     vec_float4 recipSinAngle, scale0, scale1, cosAngle, angle;
 563     vec_uint4 selectMask;
 564     cosAngle = vmathSoaV4Dot( unitVec0, unitVec1 );
 565     selectMask = (vec_uint4)vec_cmpgt( (vec_float4){_VECTORMATH_SLERP_TOL,_VECTORMATH_SLERP_TOL,_VECTORMATH_SLERP_TOL,_VECTORMATH_SLERP_TOL}, cosAngle );
 566     angle = acosf4( cosAngle );
 567     recipSinAngle = divf4( ((vec_float4){1.0f,1.0f,1.0f,1.0f}), sinf4( angle ) );
 568     scale0 = vec_sel( vec_sub( ((vec_float4){1.0f,1.0f,1.0f,1.0f}), t ), vec_madd( sinf4( vec_madd( vec_sub( ((vec_float4){1.0f,1.0f,1.0f,1.0f}), t ), angle, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) ) ), recipSinAngle, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) ), selectMask );
 569     scale1 = vec_sel( t, vec_madd( sinf4( vec_madd( t, angle, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) ) ), recipSinAngle, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) ), selectMask );
 570     vmathSoaV4ScalarMul( &tmpV4_0, unitVec0, scale0 );
 571     vmathSoaV4ScalarMul( &tmpV4_1, unitVec1, scale1 );
 572     vmathSoaV4Add( result, &tmpV4_0, &tmpV4_1 );
 573 }
 574
 575 static inline void vmathSoaV4Get4Aos( const VmathSoaVector4 *vec, VmathVector4 *result0, VmathVector4 *result1, VmathVector4 *result2, VmathVector4 *result3 )
 576 {
 577     vec_float4 tmp0, tmp1, tmp2, tmp3;
 578     tmp0 = vec_mergeh( vec->x, vec->z );
 579     tmp1 = vec_mergeh( vec->y, vec->w );
 580     tmp2 = vec_mergel( vec->x, vec->z );
 581     tmp3 = vec_mergel( vec->y, vec->w );
 582     vmathV4MakeFrom128( result0, vec_mergeh( tmp0, tmp1 ) );
 583     vmathV4MakeFrom128( result1, vec_mergel( tmp0, tmp1 ) );
 584     vmathV4MakeFrom128( result2, vec_mergeh( tmp2, tmp3 ) );
 585     vmathV4MakeFrom128( result3, vec_mergel( tmp2, tmp3 ) );
 586 }
 587
 588 static inline void vmathSoaV4StoreHalfFloats( const VmathSoaVector4 *vec, vec_ushort8 *twoQuads )
 589 {
 590     VmathVector4 v0, v1, v2, v3;
 591     vmathSoaV4Get4Aos( vec, &v0, &v1, &v2, &v3 );
 592     twoQuads[0] = _vmath2VfToHalfFloats(v0.vec128, v1.vec128);
 593     twoQuads[1] = _vmath2VfToHalfFloats(v2.vec128, v3.vec128);
 594 }
 595
 596 static inline void vmathSoaV4SetXYZ( VmathSoaVector4 *result, const VmathSoaVector3 *vec )
 597 {
 598     result->x = vec->x;
 599     result->y = vec->y;
 600     result->z = vec->z;
 601 }
 602
 603 static inline void vmathSoaV4GetXYZ( VmathSoaVector3 *result, const VmathSoaVector4 *vec )
 604 {
 605     vmathSoaV3MakeFromElems( result, vec->x, vec->y, vec->z );
 606 }
 607
 608 static inline void vmathSoaV4SetX( VmathSoaVector4 *result, vec_float4 _x )
 609 {
 610     result->x = _x;
 611 }
 612
 613 static inline vec_float4 vmathSoaV4GetX( const VmathSoaVector4 *vec )
 614 {
 615     return vec->x;
 616 }
 617
 618 static inline void vmathSoaV4SetY( VmathSoaVector4 *result, vec_float4 _y )
 619 {
 620     result->y = _y;
 621 }
 622
 623 static inline vec_float4 vmathSoaV4GetY( const VmathSoaVector4 *vec )
 624 {
 625     return vec->y;
 626 }
 627
 628 static inline void vmathSoaV4SetZ( VmathSoaVector4 *result, vec_float4 _z )
 629 {
 630     result->z = _z;
 631 }
 632
 633 static inline vec_float4 vmathSoaV4GetZ( const VmathSoaVector4 *vec )
 634 {
 635     return vec->z;
 636 }
 637
 638 static inline void vmathSoaV4SetW( VmathSoaVector4 *result, vec_float4 _w )
 639 {
 640     result->w = _w;
 641 }
 642
 643 static inline vec_float4 vmathSoaV4GetW( const VmathSoaVector4 *vec )
 644 {
 645     return vec->w;
 646 }
 647
 648 static inline void vmathSoaV4SetElem( VmathSoaVector4 *result, int idx, vec_float4 value )
 649 {
 650     *(&result->x + idx) = value;
 651 }
 652
 653 static inline vec_float4 vmathSoaV4GetElem( const VmathSoaVector4 *vec, int idx )
 654 {
 655     return *(&vec->x + idx);
 656 }
 657
 658 static inline void vmathSoaV4Add( VmathSoaVector4 *result, const VmathSoaVector4 *vec0, const VmathSoaVector4 *vec1 )
 659 {
 660     result->x = vec_add( vec0->x, vec1->x );
 661     result->y = vec_add( vec0->y, vec1->y );
 662     result->z = vec_add( vec0->z, vec1->z );
 663     result->w = vec_add( vec0->w, vec1->w );
 664 }
 665
 666 static inline void vmathSoaV4Sub( VmathSoaVector4 *result, const VmathSoaVector4 *vec0, const VmathSoaVector4 *vec1 )
 667 {
 668     result->x = vec_sub( vec0->x, vec1->x );
 669     result->y = vec_sub( vec0->y, vec1->y );
 670     result->z = vec_sub( vec0->z, vec1->z );
 671     result->w = vec_sub( vec0->w, vec1->w );
 672 }
 673
 674 static inline void vmathSoaV4ScalarMul( VmathSoaVector4 *result, const VmathSoaVector4 *vec, vec_float4 scalar )
 675 {
 676     result->x = vec_madd( vec->x, scalar, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) );
 677     result->y = vec_madd( vec->y, scalar, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) );
 678     result->z = vec_madd( vec->z, scalar, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) );
 679     result->w = vec_madd( vec->w, scalar, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) );
 680 }
 681
 682 static inline void vmathSoaV4ScalarDiv( VmathSoaVector4 *result, const VmathSoaVector4 *vec, vec_float4 scalar )
 683 {
 684     result->x = divf4( vec->x, scalar );
 685     result->y = divf4( vec->y, scalar );
 686     result->z = divf4( vec->z, scalar );
 687     result->w = divf4( vec->w, scalar );
 688 }
 689
 690 static inline void vmathSoaV4Neg( VmathSoaVector4 *result, const VmathSoaVector4 *vec )
 691 {
 692     result->x = negatef4( vec->x );
 693     result->y = negatef4( vec->y );
 694     result->z = negatef4( vec->z );
 695     result->w = negatef4( vec->w );
 696 }
 697
 698 static inline void vmathSoaV4MulPerElem( VmathSoaVector4 *result, const VmathSoaVector4 *vec0, const VmathSoaVector4 *vec1 )
 699 {
 700     result->x = vec_madd( vec0->x, vec1->x, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) );
 701     result->y = vec_madd( vec0->y, vec1->y, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) );
 702     result->z = vec_madd( vec0->z, vec1->z, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) );
 703     result->w = vec_madd( vec0->w, vec1->w, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) );
 704 }
 705
 706 static inline void vmathSoaV4DivPerElem( VmathSoaVector4 *result, const VmathSoaVector4 *vec0, const VmathSoaVector4 *vec1 )
 707 {
 708     result->x = divf4( vec0->x, vec1->x );
 709     result->y = divf4( vec0->y, vec1->y );
 710     result->z = divf4( vec0->z, vec1->z );
 711     result->w = divf4( vec0->w, vec1->w );
 712 }
 713
 714 static inline void vmathSoaV4RecipPerElem( VmathSoaVector4 *result, const VmathSoaVector4 *vec )
 715 {
 716     result->x = divf4( ((vec_float4){1.0f,1.0f,1.0f,1.0f}), vec->x );
 717     result->y = divf4( ((vec_float4){1.0f,1.0f,1.0f,1.0f}), vec->y );
 718     result->z = divf4( ((vec_float4){1.0f,1.0f,1.0f,1.0f}), vec->z );
 719     result->w = divf4( ((vec_float4){1.0f,1.0f,1.0f,1.0f}), vec->w );
 720 }
 721
 722 static inline void vmathSoaV4SqrtPerElem( VmathSoaVector4 *result, const VmathSoaVector4 *vec )
 723 {
 724     result->x = sqrtf4( vec->x );
 725     result->y = sqrtf4( vec->y );
 726     result->z = sqrtf4( vec->z );
 727     result->w = sqrtf4( vec->w );
 728 }
 729
 730 static inline void vmathSoaV4RsqrtPerElem( VmathSoaVector4 *result, const VmathSoaVector4 *vec )
 731 {
 732     result->x = divf4( ((vec_float4){1.0f,1.0f,1.0f,1.0f}), sqrtf4( vec->x ) );
 733     result->y = divf4( ((vec_float4){1.0f,1.0f,1.0f,1.0f}), sqrtf4( vec->y ) );
 734     result->z = divf4( ((vec_float4){1.0f,1.0f,1.0f,1.0f}), sqrtf4( vec->z ) );
 735     result->w = divf4( ((vec_float4){1.0f,1.0f,1.0f,1.0f}), sqrtf4( vec->w ) );
 736 }
 737
 738 static inline void vmathSoaV4AbsPerElem( VmathSoaVector4 *result, const VmathSoaVector4 *vec )
 739 {
 740     result->x = fabsf4( vec->x );
 741     result->y = fabsf4( vec->y );
 742     result->z = fabsf4( vec->z );
 743     result->w = fabsf4( vec->w );
 744 }
 745
 746 static inline void vmathSoaV4CopySignPerElem( VmathSoaVector4 *result, const VmathSoaVector4 *vec0, const VmathSoaVector4 *vec1 )
 747 {
 748     result->x = copysignf4( vec0->x, vec1->x );
 749     result->y = copysignf4( vec0->y, vec1->y );
 750     result->z = copysignf4( vec0->z, vec1->z );
 751     result->w = copysignf4( vec0->w, vec1->w );
 752 }
 753
 754 static inline void vmathSoaV4MaxPerElem( VmathSoaVector4 *result, const VmathSoaVector4 *vec0, const VmathSoaVector4 *vec1 )
 755 {
 756     result->x = fmaxf4( vec0->x, vec1->x );
 757     result->y = fmaxf4( vec0->y, vec1->y );
 758     result->z = fmaxf4( vec0->z, vec1->z );
 759     result->w = fmaxf4( vec0->w, vec1->w );
 760 }
 761
 762 static inline vec_float4 vmathSoaV4MaxElem( const VmathSoaVector4 *vec )
 763 {
 764     vec_float4 result;
 765     result = fmaxf4( vec->x, vec->y );
 766     result = fmaxf4( vec->z, result );
 767     result = fmaxf4( vec->w, result );
 768     return result;
 769 }
 770
 771 static inline void vmathSoaV4MinPerElem( VmathSoaVector4 *result, const VmathSoaVector4 *vec0, const VmathSoaVector4 *vec1 )
 772 {
 773     result->x = fminf4( vec0->x, vec1->x );
 774     result->y = fminf4( vec0->y, vec1->y );
 775     result->z = fminf4( vec0->z, vec1->z );
 776     result->w = fminf4( vec0->w, vec1->w );
 777 }
 778
 779 static inline vec_float4 vmathSoaV4MinElem( const VmathSoaVector4 *vec )
 780 {
 781     vec_float4 result;
 782     result = fminf4( vec->x, vec->y );
 783     result = fminf4( vec->z, result );
 784     result = fminf4( vec->w, result );
 785     return result;
 786 }
 787
 788 static inline vec_float4 vmathSoaV4Sum( const VmathSoaVector4 *vec )
 789 {
 790     vec_float4 result;
 791     result = vec_add( vec->x, vec->y );
 792     result = vec_add( result, vec->z );
 793     result = vec_add( result, vec->w );
 794     return result;
 795 }
 796
 797 static inline vec_float4 vmathSoaV4Dot( const VmathSoaVector4 *vec0, const VmathSoaVector4 *vec1 )
 798 {
 799     vec_float4 result;
 800     result = vec_madd( vec0->x, vec1->x, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) );
 801     result = vec_add( result, vec_madd( vec0->y, vec1->y, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) ) );
 802     result = vec_add( result, vec_madd( vec0->z, vec1->z, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) ) );
 803     result = vec_add( result, vec_madd( vec0->w, vec1->w, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) ) );
 804     return result;
 805 }
 806
 807 static inline vec_float4 vmathSoaV4LengthSqr( const VmathSoaVector4 *vec )
 808 {
 809     vec_float4 result;
 810     result = vec_madd( vec->x, vec->x, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) );
 811     result = vec_add( result, vec_madd( vec->y, vec->y, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) ) );
 812     result = vec_add( result, vec_madd( vec->z, vec->z, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) ) );
 813     result = vec_add( result, vec_madd( vec->w, vec->w, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) ) );
 814     return result;
 815 }
 816
 817 static inline vec_float4 vmathSoaV4Length( const VmathSoaVector4 *vec )
 818 {
 819     return sqrtf4( vmathSoaV4LengthSqr( vec ) );
 820 }
 821
 822 static inline void vmathSoaV4Normalize( VmathSoaVector4 *result, const VmathSoaVector4 *vec )
 823 {
 824     vec_float4 lenSqr, lenInv;
 825     lenSqr = vmathSoaV4LengthSqr( vec );
 826     lenInv = divf4( ((vec_float4){1.0f,1.0f,1.0f,1.0f}), sqrtf4( lenSqr ) );
 827     result->x = vec_madd( vec->x, lenInv, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) );
 828     result->y = vec_madd( vec->y, lenInv, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) );
 829     result->z = vec_madd( vec->z, lenInv, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) );
 830     result->w = vec_madd( vec->w, lenInv, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) );
 831 }
 832
 833 static inline void vmathSoaV4Select( VmathSoaVector4 *result, const VmathSoaVector4 *vec0, const VmathSoaVector4 *vec1, vec_uint4 select1 )
 834 {
 835     result->x = vec_sel( vec0->x, vec1->x, select1 );
 836     result->y = vec_sel( vec0->y, vec1->y, select1 );
 837     result->z = vec_sel( vec0->z, vec1->z, select1 );
 838     result->w = vec_sel( vec0->w, vec1->w, select1 );
 839 }
 840
 841 #ifdef _VECTORMATH_DEBUG
 842
 843 static inline void vmathSoaV4Print( const VmathSoaVector4 *vec )
 844 {
 845     VmathVector4 vec0, vec1, vec2, vec3;
 846     vmathSoaV4Get4Aos( vec, &vec0, &vec1, &vec2, &vec3 );
 847     printf("slot 0:\n");
 848     vmathV4Print( &vec0 );
 849     printf("slot 1:\n");
 850     vmathV4Print( &vec1 );
 851     printf("slot 2:\n");
 852     vmathV4Print( &vec2 );
 853     printf("slot 3:\n");
 854     vmathV4Print( &vec3 );
 855 }
 856
 857 static inline void vmathSoaV4Prints( const VmathSoaVector4 *vec, const char *name )
 858 {
 859     VmathVector4 vec0, vec1, vec2, vec3;
 860     printf( "%s:\n", name );
 861     vmathSoaV4Get4Aos( vec, &vec0, &vec1, &vec2, &vec3 );
 862     printf("slot 0:\n");
 863     vmathV4Print( &vec0 );
 864     printf("slot 1:\n");
 865     vmathV4Print( &vec1 );
 866     printf("slot 2:\n");
 867     vmathV4Print( &vec2 );
 868     printf("slot 3:\n");
 869     vmathV4Print( &vec3 );
 870 }
 871
 872 #endif
 873
 874 static inline void vmathSoaP3Copy( VmathSoaPoint3 *result, const VmathSoaPoint3 *pnt )
 875 {
 876     result->x = pnt->x;
 877     result->y = pnt->y;
 878     result->z = pnt->z;
 879 }
 880
 881 static inline void vmathSoaP3MakeFromElems( VmathSoaPoint3 *result, vec_float4 _x, vec_float4 _y, vec_float4 _z )
 882 {
 883     result->x = _x;
 884     result->y = _y;
 885     result->z = _z;
 886 }
 887
 888 static inline void vmathSoaP3MakeFromV3( VmathSoaPoint3 *result, const VmathSoaVector3 *vec )
 889 {
 890     result->x = vec->x;
 891     result->y = vec->y;
 892     result->z = vec->z;
 893 }
 894
 895 static inline void vmathSoaP3MakeFromScalar( VmathSoaPoint3 *result, vec_float4 scalar )
 896 {
 897     result->x = scalar;
 898     result->y = scalar;
 899     result->z = scalar;
 900 }
 901
 902 static inline void vmathSoaP3MakeFromAos( VmathSoaPoint3 *result, const VmathPoint3 *pnt )
 903 {
 904     vec_float4 vec128 = pnt->vec128;
 905     result->x = vec_splat( vec128, 0 );
 906     result->y = vec_splat( vec128, 1 );
 907     result->z = vec_splat( vec128, 2 );
 908 }
 909
 910 static inline void vmathSoaP3MakeFrom4Aos( VmathSoaPoint3 *result, const VmathPoint3 *pnt0, const VmathPoint3 *pnt1, const VmathPoint3 *pnt2, const VmathPoint3 *pnt3 )
 911 {
 912     vec_float4 tmp0, tmp1, tmp2, tmp3;
 913     tmp0 = vec_mergeh( pnt0->vec128, pnt2->vec128 );
 914     tmp1 = vec_mergeh( pnt1->vec128, pnt3->vec128 );
 915     tmp2 = vec_mergel( pnt0->vec128, pnt2->vec128 );
 916     tmp3 = vec_mergel( pnt1->vec128, pnt3->vec128 );
 917     result->x = vec_mergeh( tmp0, tmp1 );
 918     result->y = vec_mergel( tmp0, tmp1 );
 919     result->z = vec_mergeh( tmp2, tmp3 );
 920 }
 921
 922 static inline void vmathSoaP3Lerp( VmathSoaPoint3 *result, vec_float4 t, const VmathSoaPoint3 *pnt0, const VmathSoaPoint3 *pnt1 )
 923 {
 924     VmathSoaVector3 tmpV3_0, tmpV3_1;
 925     vmathSoaP3Sub( &tmpV3_0, pnt1, pnt0 );
 926     vmathSoaV3ScalarMul( &tmpV3_1, &tmpV3_0, t );
 927     vmathSoaP3AddV3( result, pnt0, &tmpV3_1 );
 928 }
 929
 930 static inline void vmathSoaP3Get4Aos( const VmathSoaPoint3 *pnt, VmathPoint3 *result0, VmathPoint3 *result1, VmathPoint3 *result2, VmathPoint3 *result3 )
 931 {
 932     vec_float4 tmp0, tmp1;
 933     tmp0 = vec_mergeh( pnt->x, pnt->z );
 934     tmp1 = vec_mergel( pnt->x, pnt->z );
 935     vmathP3MakeFrom128( result0, vec_mergeh( tmp0, pnt->y ) );
 936     vmathP3MakeFrom128( result1, vec_perm( tmp0, pnt->y, _VECTORMATH_PERM_ZBWX ) );
 937     vmathP3MakeFrom128( result2, vec_perm( tmp1, pnt->y, _VECTORMATH_PERM_XCYX ) );
 938     vmathP3MakeFrom128( result3, vec_perm( tmp1, pnt->y, _VECTORMATH_PERM_ZDWX ) );
 939 }
 940
 941 static inline void vmathSoaP3LoadXYZArray( VmathSoaPoint3 *vec, const vec_float4 *threeQuads )
 942 {
 943     vec_float4 xyxy, yzyz, zxzx, xyzx, yzxy, zxyz;
 944     xyzx = threeQuads[0];
 945     yzxy = threeQuads[1];
 946     zxyz = threeQuads[2];
 947     xyxy = vec_sld( yzxy, xyzx, 8 );
 948     zxzx = vec_sld( xyzx, zxyz, 8 );
 949     yzyz = vec_sld( zxyz, yzxy, 8 );
 950     vmathSoaP3SetX( vec, vec_perm( xyxy, zxzx, _VECTORMATH_PERM_ZBXD ) );
 951     vmathSoaP3SetY( vec, vec_perm( xyxy, yzyz, _VECTORMATH_PERM_WCYA ) );
 952     vmathSoaP3SetZ( vec, vec_perm( zxzx, yzyz, _VECTORMATH_PERM_XDZB ) );
 953 }
 954
 955 static inline void vmathSoaP3StoreXYZArray( const VmathSoaPoint3 *vec, vec_float4 *threeQuads )
 956 {
 957     vec_float4 xyzx, yzxy, zxyz, xyxy, zxzx, yzyz;
 958     xyxy = vec_perm( vec->x, vec->y, _VECTORMATH_PERM_ZCXA );
 959     zxzx = vec_perm( vec->z, vec->x, _VECTORMATH_PERM_XBZD );
 960     yzyz = vec_perm( vec->y, vec->z, _VECTORMATH_PERM_WDYB );
 961     xyzx = vec_sld( xyxy, zxzx, 8 );
 962     yzxy = vec_sld( yzyz, xyxy, 8 );
 963     zxyz = vec_sld( zxzx, yzyz, 8 );
 964     threeQuads[0] = xyzx;
 965     threeQuads[1] = yzxy;
 966     threeQuads[2] = zxyz;
 967 }
 968
 969 static inline void vmathSoaP3StoreHalfFloats( const VmathSoaPoint3 *pnt0, const VmathSoaPoint3 *pnt1, vec_ushort8 *threeQuads )
 970 {
 971     vec_float4 xyz0[3];
 972     vec_float4 xyz1[3];
 973     vmathSoaP3StoreXYZArray( pnt0, xyz0 );
 974     vmathSoaP3StoreXYZArray( pnt1, xyz1 );
 975     threeQuads[0] = _vmath2VfToHalfFloats(xyz0[0], xyz0[1]);
 976     threeQuads[1] = _vmath2VfToHalfFloats(xyz0[2], xyz1[0]);
 977     threeQuads[2] = _vmath2VfToHalfFloats(xyz1[1], xyz1[2]);
 978 }
 979
 980 static inline void vmathSoaP3SetX( VmathSoaPoint3 *result, vec_float4 _x )
 981 {
 982     result->x = _x;
 983 }
 984
 985 static inline vec_float4 vmathSoaP3GetX( const VmathSoaPoint3 *pnt )
 986 {
 987     return pnt->x;
 988 }
 989
 990 static inline void vmathSoaP3SetY( VmathSoaPoint3 *result, vec_float4 _y )
 991 {
 992     result->y = _y;
 993 }
 994
 995 static inline vec_float4 vmathSoaP3GetY( const VmathSoaPoint3 *pnt )
 996 {
 997     return pnt->y;
 998 }
 999
1000 static inline void vmathSoaP3SetZ( VmathSoaPoint3 *result, vec_float4 _z )
1001 {
1002     result->z = _z;
1003 }
1004
1005 static inline vec_float4 vmathSoaP3GetZ( const VmathSoaPoint3 *pnt )
1006 {
1007     return pnt->z;
1008 }
1009
1010 static inline void vmathSoaP3SetElem( VmathSoaPoint3 *result, int idx, vec_float4 value )
1011 {
1012     *(&result->x + idx) = value;
1013 }
1014
1015 static inline vec_float4 vmathSoaP3GetElem( const VmathSoaPoint3 *pnt, int idx )
1016 {
1017     return *(&pnt->x + idx);
1018 }
1019
1020 static inline void vmathSoaP3Sub( VmathSoaVector3 *result, const VmathSoaPoint3 *pnt0, const VmathSoaPoint3 *pnt1 )
1021 {
1022     result->x = vec_sub( pnt0->x, pnt1->x );
1023     result->y = vec_sub( pnt0->y, pnt1->y );
1024     result->z = vec_sub( pnt0->z, pnt1->z );
1025 }
1026
1027 static inline void vmathSoaP3AddV3( VmathSoaPoint3 *result, const VmathSoaPoint3 *pnt, const VmathSoaVector3 *vec1 )
1028 {
1029     result->x = vec_add( pnt->x, vec1->x );
1030     result->y = vec_add( pnt->y, vec1->y );
1031     result->z = vec_add( pnt->z, vec1->z );
1032 }
1033
1034 static inline void vmathSoaP3SubV3( VmathSoaPoint3 *result, const VmathSoaPoint3 *pnt, const VmathSoaVector3 *vec1 )
1035 {
1036     result->x = vec_sub( pnt->x, vec1->x );
1037     result->y = vec_sub( pnt->y, vec1->y );
1038     result->z = vec_sub( pnt->z, vec1->z );
1039 }
1040
1041 static inline void vmathSoaP3MulPerElem( VmathSoaPoint3 *result, const VmathSoaPoint3 *pnt0, const VmathSoaPoint3 *pnt1 )
1042 {
1043     result->x = vec_madd( pnt0->x, pnt1->x, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) );
1044     result->y = vec_madd( pnt0->y, pnt1->y, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) );
1045     result->z = vec_madd( pnt0->z, pnt1->z, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) );
1046 }
1047
1048 static inline void vmathSoaP3DivPerElem( VmathSoaPoint3 *result, const VmathSoaPoint3 *pnt0, const VmathSoaPoint3 *pnt1 )
1049 {
1050     result->x = divf4( pnt0->x, pnt1->x );
1051     result->y = divf4( pnt0->y, pnt1->y );
1052     result->z = divf4( pnt0->z, pnt1->z );
1053 }
1054
1055 static inline void vmathSoaP3RecipPerElem( VmathSoaPoint3 *result, const VmathSoaPoint3 *pnt )
1056 {
1057     result->x = divf4( ((vec_float4){1.0f,1.0f,1.0f,1.0f}), pnt->x );
1058     result->y = divf4( ((vec_float4){1.0f,1.0f,1.0f,1.0f}), pnt->y );
1059     result->z = divf4( ((vec_float4){1.0f,1.0f,1.0f,1.0f}), pnt->z );
1060 }
1061
1062 static inline void vmathSoaP3SqrtPerElem( VmathSoaPoint3 *result, const VmathSoaPoint3 *pnt )
1063 {
1064     result->x = sqrtf4( pnt->x );
1065     result->y = sqrtf4( pnt->y );
1066     result->z = sqrtf4( pnt->z );
1067 }
1068
1069 static inline void vmathSoaP3RsqrtPerElem( VmathSoaPoint3 *result, const VmathSoaPoint3 *pnt )
1070 {
1071     result->x = divf4( ((vec_float4){1.0f,1.0f,1.0f,1.0f}), sqrtf4( pnt->x ) );
1072     result->y = divf4( ((vec_float4){1.0f,1.0f,1.0f,1.0f}), sqrtf4( pnt->y ) );
1073     result->z = divf4( ((vec_float4){1.0f,1.0f,1.0f,1.0f}), sqrtf4( pnt->z ) );
1074 }
1075
1076 static inline void vmathSoaP3AbsPerElem( VmathSoaPoint3 *result, const VmathSoaPoint3 *pnt )
1077 {
1078     result->x = fabsf4( pnt->x );
1079     result->y = fabsf4( pnt->y );
1080     result->z = fabsf4( pnt->z );
1081 }
1082
1083 static inline void vmathSoaP3CopySignPerElem( VmathSoaPoint3 *result, const VmathSoaPoint3 *pnt0, const VmathSoaPoint3 *pnt1 )
1084 {
1085     result->x = copysignf4( pnt0->x, pnt1->x );
1086     result->y = copysignf4( pnt0->y, pnt1->y );
1087     result->z = copysignf4( pnt0->z, pnt1->z );
1088 }
1089
1090 static inline void vmathSoaP3MaxPerElem( VmathSoaPoint3 *result, const VmathSoaPoint3 *pnt0, const VmathSoaPoint3 *pnt1 )
1091 {
1092     result->x = fmaxf4( pnt0->x, pnt1->x );
1093     result->y = fmaxf4( pnt0->y, pnt1->y );
1094     result->z = fmaxf4( pnt0->z, pnt1->z );
1095 }
1096
1097 static inline vec_float4 vmathSoaP3MaxElem( const VmathSoaPoint3 *pnt )
1098 {
1099     vec_float4 result;
1100     result = fmaxf4( pnt->x, pnt->y );
1101     result = fmaxf4( pnt->z, result );
1102     return result;
1103 }
1104
1105 static inline void vmathSoaP3MinPerElem( VmathSoaPoint3 *result, const VmathSoaPoint3 *pnt0, const VmathSoaPoint3 *pnt1 )
1106 {
1107     result->x = fminf4( pnt0->x, pnt1->x );
1108     result->y = fminf4( pnt0->y, pnt1->y );
1109     result->z = fminf4( pnt0->z, pnt1->z );
1110 }
1111
1112 static inline vec_float4 vmathSoaP3MinElem( const VmathSoaPoint3 *pnt )
1113 {
1114     vec_float4 result;
1115     result = fminf4( pnt->x, pnt->y );
1116     result = fminf4( pnt->z, result );
1117     return result;
1118 }
1119
1120 static inline vec_float4 vmathSoaP3Sum( const VmathSoaPoint3 *pnt )
1121 {
1122     vec_float4 result;
1123     result = vec_add( pnt->x, pnt->y );
1124     result = vec_add( result, pnt->z );
1125     return result;
1126 }
1127
1128 static inline void vmathSoaP3Scale( VmathSoaPoint3 *result, const VmathSoaPoint3 *pnt, vec_float4 scaleVal )
1129 {
1130     VmathSoaPoint3 tmpP3_0;
1131     vmathSoaP3MakeFromScalar( &tmpP3_0, scaleVal );
1132     vmathSoaP3MulPerElem( result, pnt, &tmpP3_0 );
1133 }
1134
1135 static inline void vmathSoaP3NonUniformScale( VmathSoaPoint3 *result, const VmathSoaPoint3 *pnt, const VmathSoaVector3 *scaleVec )
1136 {
1137     VmathSoaPoint3 tmpP3_0;
1138     vmathSoaP3MakeFromV3( &tmpP3_0, scaleVec );
1139     vmathSoaP3MulPerElem( result, pnt, &tmpP3_0 );
1140 }
1141
1142 static inline vec_float4 vmathSoaP3Projection( const VmathSoaPoint3 *pnt, const VmathSoaVector3 *unitVec )
1143 {
1144     vec_float4 result;
1145     result = vec_madd( pnt->x, unitVec->x, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) );
1146     result = vec_add( result, vec_madd( pnt->y, unitVec->y, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) ) );
1147     result = vec_add( result, vec_madd( pnt->z, unitVec->z, ((vec_float4){0.0f,0.0f,0.0f,0.0f}) ) );
1148     return result;
1149 }
1150
1151 static inline vec_float4 vmathSoaP3DistSqrFromOrigin( const VmathSoaPoint3 *pnt )
1152 {
1153     VmathSoaVector3 tmpV3_0;
1154     vmathSoaV3MakeFromP3( &tmpV3_0, pnt );
1155     return vmathSoaV3LengthSqr( &tmpV3_0 );
1156 }
1157
1158 static inline vec_float4 vmathSoaP3DistFromOrigin( const VmathSoaPoint3 *pnt )
1159 {
1160     VmathSoaVector3 tmpV3_0;
1161     vmathSoaV3MakeFromP3( &tmpV3_0, pnt );
1162     return vmathSoaV3Length( &tmpV3_0 );
1163 }
1164
1165 static inline vec_float4 vmathSoaP3DistSqr( const VmathSoaPoint3 *pnt0, const VmathSoaPoint3 *pnt1 )
1166 {
1167     VmathSoaVector3 tmpV3_0;
1168     vmathSoaP3Sub( &tmpV3_0, pnt1, pnt0 );
1169     return vmathSoaV3LengthSqr( &tmpV3_0 );
1170 }
1171
1172 static inline vec_float4 vmathSoaP3Dist( const VmathSoaPoint3 *pnt0, const VmathSoaPoint3 *pnt1 )
1173 {
1174     VmathSoaVector3 tmpV3_0;
1175     vmathSoaP3Sub( &tmpV3_0, pnt1, pnt0 );
1176     return vmathSoaV3Length( &tmpV3_0 );
1177 }
1178
1179 static inline void vmathSoaP3Select( VmathSoaPoint3 *result, const VmathSoaPoint3 *pnt0, const VmathSoaPoint3 *pnt1, vec_uint4 select1 )
1180 {
1181     result->x = vec_sel( pnt0->x, pnt1->x, select1 );
1182     result->y = vec_sel( pnt0->y, pnt1->y, select1 );
1183     result->z = vec_sel( pnt0->z, pnt1->z, select1 );
1184 }
1185
1186 #ifdef _VECTORMATH_DEBUG
1187
1188 static inline void vmathSoaP3Print( const VmathSoaPoint3 *pnt )
1189 {
1190     VmathPoint3 vec0, vec1, vec2, vec3;
1191     vmathSoaP3Get4Aos( pnt, &vec0, &vec1, &vec2, &vec3 );
1192     printf("slot 0:\n");
1193     vmathP3Print( &vec0 );
1194     printf("slot 1:\n");
1195     vmathP3Print( &vec1 );
1196     printf("slot 2:\n");
1197     vmathP3Print( &vec2 );
1198     printf("slot 3:\n");
1199     vmathP3Print( &vec3 );
1200 }
1201
1202 static inline void vmathSoaP3Prints( const VmathSoaPoint3 *pnt, const char *name )
1203 {
1204     VmathPoint3 vec0, vec1, vec2, vec3;
1205     printf( "%s:\n", name );
1206     vmathSoaP3Get4Aos( pnt, &vec0, &vec1, &vec2, &vec3 );
1207     printf("slot 0:\n");
1208     vmathP3Print( &vec0 );
1209     printf("slot 1:\n");
1210     vmathP3Print( &vec1 );
1211     printf("slot 2:\n");
1212     vmathP3Print( &vec2 );
1213     printf("slot 3:\n");
1214     vmathP3Print( &vec3 );
1215 }
1216
1217 #endif
1218
1219 #ifdef __cplusplus
1220 }
1221 #endif /* __cplusplus */
1222
1223 #endif