开通博客赚积分
发布资源赚积分
分类

源码开发语言/平台
当前位置：首页 > 源码/资料 > 游戏 > 其他游戏 > 查看源码
hkSseVector4.inl
资源名称：wow-313-source.rar　[点击查看]
上传用户：yisoukefu
上传日期：2020-08-09
资源大小：39506k
文件大小：20k
源码类别：

其他游戏

开发平台：

Visual C++

hkSseVector4.inl：源码内容
  1. /* 
  2.  * 
  3.  * Confidential Information of Telekinesys Research Limited (t/a Havok). Not for disclosure or distribution without Havok's
  4.  * prior written consent. This software contains code, techniques and know-how which is confidential and proprietary to Havok.
  5.  * Level 2 and Level 3 source code contains trade secrets of Havok. Havok Software (C) Copyright 1999-2009 Telekinesys Research Limited t/a Havok. All Rights Reserved. Use of this software is subject to the terms of an end user license agreement.
  6.  * 
  7.  */
  10. /* quad, here for inlining */
  12. inline hkQuadReal& hkVector4::getQuad()
  13. {
  14. return m_quad;
  15. }
  17. inline const hkQuadReal& hkVector4::getQuad() const
  18. {
  19. return m_quad;
  20. }
  22. #ifndef HK_DISABLE_VECTOR4_CONSTRUCTORS
  23. /* construct, assign, zero */
  24. inline hkVector4::hkVector4(hkReal a, hkReal b, hkReal c, hkReal d)
  25. {
  26. m_quad = _mm_setr_ps(a,b,c,d);
  27. }
  29. inline hkVector4::hkVector4(const hkQuadReal& q)
  30. {
  31. m_quad = q;
  32. }
  34. inline hkVector4::hkVector4(const hkVector4& v)
  35. {
  36. m_quad = v.m_quad;
  37. }
  38. #endif
  40. inline void hkVector4::operator= (const hkVector4& v)
  41. {
  42. m_quad = v.m_quad;
  43. }
  46. inline void hkVector4::set(hkReal a, hkReal b, hkReal c, hkReal d)
  47. {
  48. m_quad = _mm_setr_ps(a,b,c,d);
  49. }
  51. inline void hkVector4::setAll(hkSimdRealParameter a)
  52. {
  53. m_quad = _mm_shuffle_ps( a.getQuad(), a.getQuad(), _MM_SHUFFLE(0,0,0,0));
  54. }
  56. inline void hkVector4::setAll(hkReal a)
  57. {
  58. m_quad = _mm_set_ps1(a);
  59. }
  61. inline void hkVector4::setAll3(hkReal a)
  62. {
  63. m_quad = _mm_set_ps1(a);
  64. }
  66. inline void hkVector4::setZero4()
  67. {
  68. m_quad = _mm_setzero_ps();
  69. }
  71. void hkVector4::setInt24W( int value )
  72. {
  73. (reinterpret_cast<hkUint32*>(this))[3] = value |= 0x3f000000;
  74. }
  76. // Gets the .w component as an integer with 24 bit resolution (for real experts only)
  77. int hkVector4::getInt24W( ) const 
  78. {
  79. return (reinterpret_cast<const hkUint32*>(this))[3] & ~0x3f000000;
  80. }
  82. inline void hkVector4::storeUncached( void* dest) const
  83. {
  84. _mm_stream_ps( (float*)dest, this->m_quad );
  85. }
  88. inline void hkVector4::zeroElement( int i )
  89. {
  90. HK_ASSERT(0x3bc36625, i>=0 && i<4);
  91. static HK_ALIGN16( const hkUint32 cx[4][4] ) ={ 
  92. { 0x00000000, 0xffffffff, 0xffffffff, 0xffffffff },
  93. { 0xffffffff, 0x00000000, 0xffffffff, 0xffffffff },
  94. { 0xffffffff, 0xffffffff, 0x00000000, 0xffffffff },
  95. { 0xffffffff, 0xffffffff, 0xffffffff, 0x00000000 } };
  96. m_quad = _mm_and_ps( m_quad, reinterpret_cast<const hkQuadReal*>(cx)[i] );
  97. }
  100. /* vector methods */
  102. inline void hkVector4::add4(const hkVector4& v)
  103. {
  104. m_quad = _mm_add_ps( m_quad, v.m_quad );
  105. }
  106. inline void hkVector4::add3clobberW(const hkVector4& v)
  107. {
  108. m_quad = _mm_add_ps( m_quad, v.m_quad );
  109. }
  111. inline void hkVector4::setAdd4(const hkVector4& v0, const hkVector4& v1)
  112. {
  113. m_quad = _mm_add_ps(v0.m_quad, v1.m_quad);
  114. }
  116. inline void hkVector4::sub4(const hkVector4& v)
  117. {
  118. m_quad = _mm_sub_ps( m_quad, v.m_quad );
  119. }
  121. inline void hkVector4::sub3clobberW(const hkVector4& v)
  122. {
  123. m_quad = _mm_sub_ps( m_quad, v.m_quad );
  124. }
  126. inline void hkVector4::setSub4(const hkVector4& v0, const hkVector4& v1)
  127. {
  128. m_quad = _mm_sub_ps(v0.m_quad, v1.m_quad);
  129. }
  131. inline void hkVector4::mul4(const hkVector4& v)
  132. {              
  133. m_quad = _mm_mul_ps( m_quad, v.m_quad );
  134. }
  136. inline void hkVector4::setNeg3(const hkVector4& v)
  137. {
  138. static HK_ALIGN16( const hkUint32 negateMask[4] ) = { 0x80000000, 0x80000000, 0x80000000, 0x00000000 };
  139. m_quad = _mm_xor_ps(v.m_quad, *(const hkQuadReal*)&negateMask);
  140. }
  143. inline void hkVector4::setMul4(const hkVector4& a, const hkVector4& b)
  144. {
  145. m_quad = _mm_mul_ps( a.m_quad, b.m_quad );
  146. }
  148. inline void hkVector4::mul4(hkSimdRealParameter s)
  149. {              
  150. m_quad = _mm_mul_ps( s.broadcast(), m_quad );
  151. }
  153. inline void hkVector4::setMul4(hkSimdRealParameter r, const hkVector4& v1)
  154. {
  155. m_quad = _mm_mul_ps( r.broadcast(), v1.m_quad);
  156. }
  158. inline void hkVector4::addMul4(hkSimdRealParameter r, const hkVector4& v1)
  159. {
  160. m_quad = _mm_add_ps( m_quad,
  161. _mm_mul_ps( r.broadcast(), v1.m_quad) );
  162. }
  164. inline void hkVector4::addMul4(const hkVector4& x, const hkVector4& y)
  165. {
  166. m_quad = _mm_add_ps( m_quad,
  167. _mm_mul_ps( x.m_quad, y.m_quad) );
  168. }
  170. inline void hkVector4::subMul4(hkSimdRealParameter r, const hkVector4& v1)
  171. {
  172. m_quad = _mm_sub_ps( m_quad,
  173. _mm_mul_ps( r.broadcast(), v1.m_quad) );
  174. }
  176. inline void hkVector4::subMul4(const hkVector4& x, const hkVector4& y)
  177. {
  178. m_quad = _mm_sub_ps( m_quad,
  179. _mm_mul_ps( x.m_quad, y.m_quad) );
  180. }
  182. inline void hkVector4::setAddMul4(const hkVector4& a, const hkVector4& b, hkSimdRealParameter r)
  183. {
  184. m_quad = _mm_add_ps( a.m_quad,
  185.  _mm_mul_ps( r.broadcast(), b.m_quad) );
  186. }
  188. inline void hkVector4::setAddMul4(const hkVector4& a, const hkVector4& x, const hkVector4& y)
  189. {
  190. m_quad = _mm_add_ps( a.m_quad,
  191.  _mm_mul_ps( x.m_quad, y.m_quad) );
  192. }
  194. inline void hkVector4::setCross( const hkVector4& v0, const hkVector4& v1 )
  195. {
  196. hkQuadReal cross0;
  197. cross0 = _mm_mul_ps(
  198. _mm_shuffle_ps(v0.m_quad, v0.m_quad, _MM_SHUFFLE(3,0,2,1)),
  199. _mm_shuffle_ps(v1.m_quad, v1.m_quad, _MM_SHUFFLE(3,1,0,2)) );
  200. hkQuadReal cross1;
  201. cross1 = _mm_mul_ps(
  202. _mm_shuffle_ps(v0.m_quad, v0.m_quad, _MM_SHUFFLE(3,1,0,2)),
  203. _mm_shuffle_ps(v1.m_quad, v1.m_quad, _MM_SHUFFLE(3,0,2,1)) );
  204. m_quad = _mm_sub_ps(cross0, cross1);
  205. }
  207. inline void hkVector4::setInterpolate4(const hkVector4& v0, const hkVector4& v1, hkSimdRealParameter _t)
  208. {
  209. static HK_ALIGN16( const float cw[4] ) = { 1.0f, 1.0f, 1.0f, 1.0f };
  210. hkQuadReal t = _t.broadcast();
  211. hkQuadReal s = _mm_sub_ps( *(const hkQuadReal*)&cw, t);
  212. m_quad = _mm_add_ps(
  213. _mm_mul_ps(s, v0.m_quad),
  214. _mm_mul_ps(t, v1.m_quad) );
  215. }
  217. inline hkVector4Comparison hkVector4::compareLessThan4(hkVector4Parameter a) const
  218. {
  219. hkVector4Comparison m; m.m_mask =_mm_cmplt_ps(m_quad, a.m_quad);
  220. return m;
  221. }
  223. inline hkVector4Comparison hkVector4::compareEqual4(hkVector4Parameter a) const
  224. {
  225. hkVector4Comparison m; m.m_mask =_mm_cmpeq_ps(m_quad, a.m_quad);
  226. return m;
  227. }
  229. inline hkVector4Comparison hkVector4::compareLessThanEqual4(const hkVector4 &a) const
  230. {
  231. hkVector4Comparison m; m.m_mask =_mm_cmple_ps(m_quad, a.m_quad);
  232. return m;
  233. }
  235. inline hkVector4Comparison hkVector4::compareLessThanZero4() const
  236. {
  237. hkVector4 zero; zero.setZero4();
  238. hkVector4Comparison m; m.m_mask =_mm_cmplt_ps(m_quad, zero);
  239. return m;
  240. }
  242. inline hkBool32 hkVector4::allLessThan3(hkVector4Parameter a) const
  243. {
  244. return (_mm_movemask_ps( _mm_cmplt_ps(m_quad, a.m_quad) ) & hkVector4Comparison::MASK_XYZ) == hkVector4Comparison::MASK_XYZ;
  245. }
  247. inline hkBool32 hkVector4::allLessThan4(hkVector4Parameter a) const
  248. {
  249. return _mm_movemask_ps( _mm_cmplt_ps(m_quad, a.m_quad) ) == hkVector4Comparison::MASK_XYZW;
  250. }
  252. inline void hkVector4::select32( hkVector4Parameter a, hkVector4Parameter b, const hkVector4Comparison& comp)
  253. {
  254. m_quad = _mm_or_ps( _mm_and_ps(comp.m_mask, b.m_quad), _mm_andnot_ps(comp.m_mask, a.m_quad) );
  255. }
  257. inline void hkVector4::setNeg4(const hkVector4& v)
  258. {
  259. static HK_ALIGN16( const hkUint32 negateMask[4] ) = { 0x80000000, 0x80000000, 0x80000000, 0x80000000 };
  260. m_quad = _mm_xor_ps(v.m_quad, *(const hkQuadReal*)&negateMask);
  261. }
  263. inline void hkVector4::setAbs4(const hkVector4& v)
  264. {
  265. static HK_ALIGN16( const hkUint32 absMask[4] ) = { 0x7fffffff, 0x7fffffff, 0x7fffffff, 0x7fffffff };
  266. m_quad = _mm_and_ps(v.m_quad, *(const hkQuadReal*)&absMask);
  267. }
  269. inline void hkVector4::setMin4(const hkVector4& a, const hkVector4& b)
  270. {
  271. m_quad = _mm_min_ps(a.getQuad(), b.getQuad());
  272. }
  274. inline void hkVector4::setMax4(const hkVector4& a, const hkVector4& b)
  275. {
  276. m_quad = _mm_max_ps(a.getQuad(), b.getQuad());
  277. }
  279. /* matrix3, rotation, quaternion, transform */
  281. inline void hkVector4::_setMul3(const hkMatrix3& a, const hkVector4& b )
  282. {
  283. const hkQuadReal b0 = _mm_shuffle_ps( b.m_quad, b.m_quad, _MM_SHUFFLE(0,0,0,0));
  284. const hkQuadReal r0 = _mm_mul_ps( a.getColumn(0).m_quad, b0 );
  285. const hkQuadReal b1 = _mm_shuffle_ps( b.m_quad, b.m_quad, _MM_SHUFFLE(1,1,1,1));
  286. const hkQuadReal r1 = _mm_mul_ps( a.getColumn(1).m_quad, b1 );
  287. const hkQuadReal b2 = _mm_shuffle_ps( b.m_quad, b.m_quad, _MM_SHUFFLE(2,2,2,2));
  288. const hkQuadReal r2 = _mm_mul_ps( a.getColumn(2).m_quad, b2 );
  290. m_quad = _mm_add_ps( _mm_add_ps(r0, r1), r2 );
  291. }
  293. inline void hkVector4::_setMul4(const hkMatrix3& r, const hkVector4& v)
  294. {
  295. _setMul3(r,v);
  296. }
  299. inline void hkVector4::_setRotatedDir (const hkRotation& r, const hkVector4& v)
  300. {
  301. _setMul3(r,v);
  302. }
  304. inline void hkVector4::_setRotatedInverseDir(const hkRotation& r, const hkVector4& b)
  305. {
  309. // - horizontal add is 15+a few more shuffles.
  310. hkQuadReal c0 = r.getColumn(0).m_quad;
  311. hkQuadReal c1 = r.getColumn(1).m_quad;
  312. hkQuadReal c2 = r.getColumn(2).m_quad;
  313. HK_TRANSPOSE3(c0,c1,c2);
  315. const hkQuadReal b0 = _mm_shuffle_ps( b.m_quad, b.m_quad, _MM_SHUFFLE(0,0,0,0));
  316. const hkQuadReal r0 = _mm_mul_ps( c0, b0 );
  317. const hkQuadReal b1 = _mm_shuffle_ps( b.m_quad, b.m_quad, _MM_SHUFFLE(1,1,1,1));
  318. const hkQuadReal r1 = _mm_mul_ps( c1, b1 );
  319. const hkQuadReal b2 = _mm_shuffle_ps( b.m_quad, b.m_quad, _MM_SHUFFLE(2,2,2,2));
  320. const hkQuadReal r2 = _mm_mul_ps( c2, b2 );
  322. m_quad = _mm_add_ps( _mm_add_ps(r0, r1), r2 );
  323. }
  326. inline void hkVector4::_setTransformedPos(const hkTransform& t, const hkVector4& b)
  327. {
  328. const hkRotation& r = t.getRotation();
  329. hkQuadReal b0 = _mm_shuffle_ps( b.m_quad, b.m_quad, _MM_SHUFFLE(0,0,0,0));
  330. hkQuadReal r0 = _mm_mul_ps( r.getColumn(0).m_quad, b0 );
  331. hkQuadReal b1 = _mm_shuffle_ps( b.m_quad, b.m_quad, _MM_SHUFFLE(1,1,1,1) );
  332. hkQuadReal r1 = _mm_mul_ps( r.getColumn(1).m_quad, b1 );
  333. hkQuadReal b2 = _mm_shuffle_ps( b.m_quad, b.m_quad, _MM_SHUFFLE(2,2,2,2));
  334. hkQuadReal r2 = _mm_mul_ps( r.getColumn(2).m_quad, b2 );
  335. m_quad = _mm_add_ps( _mm_add_ps(r0, r1), _mm_add_ps(r2, t.getTranslation().m_quad ) );
  336. }
  338. inline void hkVector4::_setMul4xyz1(const hkTransform& t, const hkVector4& v)
  339. {
  340. _setTransformedPos( t, v );
  341. }
  345. inline void hkVector4::_setTransformedInversePos(const hkTransform& a, const hkVector4& b)
  346. {
  347. hkVector4 t0; t0.setSub4( b, a.getTranslation() );
  348. _setRotatedInverseDir(a.getRotation(), t0);
  349. }
  351. inline hkSimdReal hkVector4::dot3(const hkVector4& a) const
  352. {
  353. hkQuadReal x2 = _mm_mul_ps(m_quad,a.m_quad);
  354. hkQuadReal xySum = _mm_add_ss( _mm_shuffle_ps(x2,x2,_MM_SHUFFLE(1,1,1,1)), x2);
  355. return _mm_add_ss( _mm_shuffle_ps(x2,x2,_MM_SHUFFLE(2,2,2,2)), xySum);
  356. }
  358. inline void hkVector4::setDot3(hkVector4Parameter a, hkVector4Parameter b)
  359. {
  360. hkQuadReal x2 = _mm_mul_ps(a.m_quad, b.m_quad);
  361. hkQuadReal xySum = _mm_add_ss( _mm_shuffle_ps(x2,x2,_MM_SHUFFLE(1,1,1,1)), x2);
  362. m_quad = _mm_add_ss( _mm_shuffle_ps(x2,x2,_MM_SHUFFLE(2,2,2,2)), xySum);
  363. }
  365. inline hkReal hkVector4::dot3fpu(const hkVector4& a) const
  366. {
  367. const hkVector4& t = *this;
  368. return (t(0) * a(0)) + ( t(1) * a(1)) + ( t(2) * a(2) );
  369. }
  371. inline hkSimdReal hkVector4::dot4(const hkVector4& a) const
  372. {
  373. hkQuadReal x2 = _mm_mul_ps(m_quad,a.m_quad);
  374. hkQuadReal sum0 = _mm_add_ps( _mm_shuffle_ps(x2,x2,_MM_SHUFFLE(1,0,3,2)), x2); // w+z w+z x+y x+y
  375. hkQuadReal sum1 = _mm_shuffle_ps(sum0,sum0, _MM_SHUFFLE(2,3,0,1)); // x+y x+y w+z w+z
  376. return _mm_add_ps( sum0, sum1 ); // x+y+w+z all 4
  377. }
  378. inline void hkVector4::setDot4(hkVector4Parameter a, hkVector4Parameter b)
  379. {
  380. hkQuadReal x2 = _mm_mul_ps(a.m_quad, b.m_quad);
  381. hkQuadReal sum0 = _mm_add_ps( _mm_shuffle_ps(x2,x2,_MM_SHUFFLE(1,0,3,2)), x2); // w+z w+z x+y x+y
  382. hkQuadReal sum1 = _mm_shuffle_ps(sum0,sum0, _MM_SHUFFLE(2,3,0,1)); // x+y x+y w+z w+z
  383. m_quad = _mm_add_ps( sum0, sum1 ); // x+y+w+z all 4
  384. }
  386. inline hkSimdReal hkVector4::dot4xyz1(const hkVector4& a) const
  387. {
  388. hkQuadReal x2 = _mm_mul_ps(m_quad,a.m_quad);
  389. hkQuadReal xySum = _mm_add_ss( _mm_shuffle_ps(x2,x2,_MM_SHUFFLE(1,1,1,1)), x2);
  390. hkQuadReal zwSum = _mm_add_ss( _mm_shuffle_ps(x2,x2,_MM_SHUFFLE(2,2,2,2)), _mm_shuffle_ps(m_quad, m_quad, _MM_SHUFFLE(3,3,3,3)));
  391. return _mm_add_ps( xySum, zwSum ); // x+y+w+z all 4
  392. }
  394. inline hkSimdReal hkVector4::horizontalAdd3() const
  395. {
  396. hkQuadReal xySum = _mm_add_ss( _mm_shuffle_ps(m_quad,m_quad,_MM_SHUFFLE(1,1,1,1)), m_quad);
  397. return _mm_add_ss( _mm_shuffle_ps(m_quad,m_quad,_MM_SHUFFLE(2,2,2,2)), xySum);
  398. }
  400. inline hkSimdReal hkVector4::horizontalAdd4() const
  401. {
  402. hkQuadReal sum0 = _mm_add_ps( _mm_shuffle_ps(m_quad,m_quad,_MM_SHUFFLE(1,0,3,2)), m_quad); // w+z w+z x+y x+y
  403. hkQuadReal sum1 = _mm_shuffle_ps(sum0,sum0, _MM_SHUFFLE(2,3,0,1)); // x+y x+y w+z w+z
  404. return _mm_add_ps( sum0, sum1 ); // x+y+w+z all 4
  405. }
  407. inline hkSimdReal hkVector4::length3() const
  408. {
  409. return _mm_sqrt_ss( this->dot3(*this).getQuad() );
  410. }
  412. inline hkSimdReal hkVector4::lengthSquared3() const
  413. {
  414. return this->dot3(*this);
  415. }
  417. inline hkSimdReal hkVector4::lengthInverse3() const
  418. {
  419. //HK_ASSERT2(0xf0435654, this->lengthSquared3() > 0, "hkVector4 too small for lengthInverse3");
  421. const hkQuadReal half  = _mm_set_ss(0.5f);
  422. const hkQuadReal three = _mm_set_ss(3.0f);
  423. const hkQuadReal zero  = _mm_set_ss(0.0f);
  425. const hkQuadReal len2 = this->dot3(*this).getQuad();
  427. const hkQuadReal len2EqualsZero = _mm_cmpeq_ss(len2, zero);
  429. const hkQuadReal approx = _mm_rsqrt_ss(len2);
  430. const hkQuadReal refined = _mm_mul_ss(
  431. _mm_mul_ss(half, approx),
  432. _mm_sub_ss(three, _mm_mul_ss( _mm_mul_ss(len2, approx), approx ) ) );
  435. // Ensures that we return 0 if the length is 0
  436. return _mm_andnot_ps(len2EqualsZero, refined);
  437. }
  439. inline hkSimdReal hkVector4::length4() const
  440. {
  441. return _mm_sqrt_ss( this->dot4(*this).getQuad() );
  442. }
  444. inline hkSimdReal hkVector4::lengthSquared4() const
  445. {
  446. return this->dot4(*this);
  447. }
  449. inline hkSimdReal hkVector4::lengthInverse4() const
  450. {
  451. const hkQuadReal half  = _mm_set_ss(0.5f);
  452. const hkQuadReal three = _mm_set_ss(3.0f);
  453. const hkQuadReal zero  = _mm_set_ss(0.0f);
  455. const hkQuadReal len2 = this->dot4(*this).getQuad();
  456. const hkQuadReal len2EqualsZero = _mm_cmpeq_ss(len2, zero);
  458. const hkQuadReal approx = _mm_rsqrt_ss(len2);
  459. const hkQuadReal refined = _mm_mul_ss(
  460. _mm_mul_ss(half, approx),
  461. _mm_sub_ss(three, _mm_mul_ss( _mm_mul_ss(len2, approx), approx ) ) );
  463. // Ensures that we return 0 if the length is 0
  464. return _mm_andnot_ps(len2EqualsZero, refined);
  465. }
  467. inline void hkVector4::normalize3()
  468. {
  469. hkQuadReal lenInv = lengthInverse3().broadcast();
  470. m_quad = _mm_mul_ps(lenInv, m_quad);
  471. //HK_ASSERT2(0xf0435f54, this->lengthSquared3() > 0.5f, "hkVector4 cannot be normalized");
  472. }
  474. inline hkSimdReal hkVector4::normalizeWithLength3()
  475. {
  476. HK_ASSERT2(0xf043f654, this->lengthSquared3() > 0, "hkVector4 too small for lengthInverse3");
  478. const hkQuadReal half  = _mm_set_ss(0.5f);
  479. const hkQuadReal three = _mm_set_ss(3.0f);
  481. hkQuadReal len2 = this->dot3(*this).getQuad();
  482. hkQuadReal approx = _mm_rsqrt_ss(len2);
  483. hkQuadReal lenInv = _mm_mul_ss(
  484. _mm_mul_ss(half, approx),
  485. _mm_sub_ss(three, _mm_mul_ss( _mm_mul_ss(len2, approx), approx ) ) );
  486. lenInv = _mm_shuffle_ps(lenInv, lenInv, _MM_SHUFFLE(0,0,0,0) );
  487. m_quad = _mm_mul_ps(lenInv, m_quad);
  488. return _mm_mul_ss(len2, lenInv); // quicker to return x^2/x
  489. }
  491. inline void hkVector4::normalize4()
  492. {
  493. hkQuadReal lenInv = lengthInverse4().broadcast();
  494. m_quad = _mm_mul_ps(lenInv, m_quad);
  495. }
  497. inline hkSimdReal hkVector4::normalizeWithLength4()
  498. {
  499. const hkQuadReal half  = _mm_set_ss(0.5f);
  500. const hkQuadReal three = _mm_set_ss(3.0f);
  502. hkQuadReal len2 = this->dot4(*this).getQuad();
  503. hkQuadReal approx = _mm_rsqrt_ss(len2);
  504. hkQuadReal lenInv = _mm_mul_ss(
  505. _mm_mul_ss(half, approx),
  506. _mm_sub_ss(three, _mm_mul_ss( _mm_mul_ss(len2, approx), approx ) ) );
  507. lenInv = _mm_shuffle_ps(lenInv, lenInv, _MM_SHUFFLE(0,0,0,0) );
  508. m_quad = _mm_mul_ps(lenInv, m_quad);
  509. return _mm_mul_ss(len2, lenInv); // quicker to return x^2/x
  510. }
  513. inline void hkVector4::fastNormalize3()
  514. {
  515. hkQuadReal len2 = this->dot3(*this).getQuad();
  516. hkSimdReal approx = _mm_rsqrt_ss(len2);
  517. m_quad = _mm_mul_ps(m_quad, approx.broadcast());
  518. }
  520. inline void hkVector4::fastNormalize3NonZero()
  521. {
  522. hkQuadReal len2 = this->dot3(*this).getQuad();
  523. hkSimdReal approx = _mm_rsqrt_ss(len2);
  524. m_quad = _mm_mul_ps(m_quad, approx.broadcast());
  525. }
  527. inline hkSimdReal hkVector4::fastNormalizeWithLength3()
  528. {
  529. hkQuadReal len2 = this->dot3(*this).getQuad();
  530. hkQuadReal approx = _mm_rsqrt_ss(len2);
  531. approx = _mm_shuffle_ps(approx, approx, _MM_SHUFFLE(0,0,0,0) );
  532. m_quad = _mm_mul_ps(m_quad, approx);
  533. return _mm_mul_ss(len2, approx);
  534. }
  536. /* operator () */
  538. inline hkReal& hkVector4::operator() (int a)
  539. {
  540. return reinterpret_cast<hkReal*>(&m_quad)[a];
  541. }
  542. inline const hkReal& hkVector4::operator() (int a) const
  543. {
  544. return reinterpret_cast<const hkReal*>(&m_quad)[a];
  545. }
  547. #define HK_VECTOR4_COMBINE_XYZ_W(xyz, w) 
  548. _mm_shuffle_ps( xyz, _mm_unpackhi_ps(xyz, w), _MM_SHUFFLE(3,0,1,0))
  550. inline void hkVector4::setXYZW(const hkVector4& xyz, const hkVector4& w)
  551. {
  552. m_quad = HK_VECTOR4_COMBINE_XYZ_W(xyz.m_quad, w.m_quad);
  553. }
  555. inline void hkVector4::setW(const hkVector4& w)
  556. {
  557. m_quad = HK_VECTOR4_COMBINE_XYZ_W(m_quad, w.m_quad);
  558. }
  560. inline void hkVector4::setXYZ(const hkVector4& xyz)
  561. {
  562. m_quad = HK_VECTOR4_COMBINE_XYZ_W(xyz.m_quad, m_quad);
  563. }
  565. inline void hkVector4::setXYZ0(const hkVector4& xyz)
  566. {
  567. static HK_ALIGN16( const hkUint32 cw[4] ) = { 0xffffffff, 0xffffffff, 0xffffffff, 0x00000000 };
  568. m_quad = _mm_and_ps( xyz, *(const hkQuadReal*)&cw );
  569. }
  571. inline void hkVector4::setReciprocal4(const hkVector4& vec)
  572. {
  573. m_quad = hkMath::quadReciprocal(vec.m_quad);
  574. }
  576. inline void hkVector4::setSqrtInverse4(const hkVector4& vec)
  577. {
  578. m_quad = hkMath::quadReciprocalSquareRoot(vec.m_quad);
  579. }
  581. inline hkSimdReal hkVector4::getSimdAt(int i) const
  582. {
  583. HK_ASSERT2(0x6d0c31d7, i>=0 && i<4, "index out of bounds for element access");
  584. switch(i)
  585. {
  586. case 0:
  587. return m_quad;
  588. case 1:
  589. return _mm_shuffle_ps(m_quad, m_quad, _MM_SHUFFLE(1,1,1,1));
  590. case 2:
  591. return _mm_shuffle_ps(m_quad, m_quad, _MM_SHUFFLE(2,2,2,2));
  592. case 3:
  593. default:
  594. return _mm_shuffle_ps(m_quad, m_quad, _MM_SHUFFLE(3,3,3,3));
  595. }
  596. }
  598. inline void hkVector4::broadcast(int i)
  599. {
  600. HK_ASSERT2(0x32b298db, i>=0 && i<4, "index out of bounds for broadcast");
  601. if( i == 0 )
  602. {
  603. m_quad = _mm_shuffle_ps(m_quad, m_quad, _MM_SHUFFLE(0,0,0,0));
  604. }
  605. else if( i == 1 )
  606. {
  607. m_quad = _mm_shuffle_ps(m_quad, m_quad, _MM_SHUFFLE(1,1,1,1));
  608. }
  609. else if( i == 2 )
  610. {
  611. m_quad = _mm_shuffle_ps(m_quad, m_quad, _MM_SHUFFLE(2,2,2,2));
  612. }
  613. else if( i == 3 )
  614. {
  615. m_quad = _mm_shuffle_ps(m_quad, m_quad, _MM_SHUFFLE(3,3,3,3));
  616. }
  617. }
  619. inline void hkVector4::setBroadcast(const hkVector4& v, int i)
  620. {
  621. HK_ASSERT2(0x7d6992c5, i>=0 && i<4, "index out of bounds for broadcast");
  622. if( i == 0 )
  623. {
  624. m_quad = _mm_shuffle_ps(v.m_quad, v.m_quad, _MM_SHUFFLE(0,0,0,0));
  625. }
  626. else if( i == 1 )
  627. {
  628. m_quad = _mm_shuffle_ps(v.m_quad, v.m_quad, _MM_SHUFFLE(1,1,1,1));
  629. }
  630. else if( i == 2 )
  631. {
  632. m_quad = _mm_shuffle_ps(v.m_quad, v.m_quad, _MM_SHUFFLE(2,2,2,2));
  633. }
  634. else if( i == 3 )
  635. {
  636. m_quad = _mm_shuffle_ps(v.m_quad, v.m_quad, _MM_SHUFFLE(3,3,3,3));
  637. }
  638. }
  640. inline void hkVector4::setBroadcast3clobberW(const hkVector4& v, int i)
  641. {
  642. setBroadcast( v, i );
  643. }
  645. inline void hkVector4::_setTransformedPos(const hkQsTransform& a, const hkVector4& b)
  646. {
  647. hkVector4 temp(b);
  648. temp.mul4(a.m_scale);
  649. setRotatedDir(a.m_rotation, temp);
  650. add4(a.m_translation);
  651. }
  653. inline void hkVector4::_setTransformedInversePos(const hkQsTransform& a, const hkVector4& b)
  654. {
  655. hkVector4 temp(b);
  656. temp.sub4(a.m_translation);
  657. setRotatedInverseDir(a.m_rotation, temp);
  659. hkVector4 invScale; invScale.setReciprocal4(a.m_scale);
  660. mul4(invScale);
  661. }
  663. #define HK_VECTOR4_load4a
  664. inline void hkVector4::load4a(const hkReal* p)
  665. {
  666. HK_ASSERT2(0x64211c2f, ( ((hkUlong)p) & 0xf ) == 0, "p must be 16-byte aligned.");
  667. m_quad = _mm_load_ps(p);
  668. }
  670. #define HK_VECTOR4_store4a
  671. inline void hkVector4::store4a(hkReal* p) const
  672. {
  673. HK_ASSERT2(0x6b865439, ( ((hkUlong)p) & 0xf ) == 0, "p must be 16-byte aligned.");
  674. _mm_store_ps(p, m_quad);
  675. }
  677. #define HK_VECTOR4_load4
  678. inline void hkVector4::load4(const hkReal* p)
  679. {
  680. m_quad = _mm_loadu_ps(p);
  681. }
  683. /*
  684. * Havok SDK - NO SOURCE PC DOWNLOAD, BUILD(#20090216)
  685. * 
  686. * Confidential Information of Havok.  (C) Copyright 1999-2009
  687. * Telekinesys Research Limited t/a Havok. All Rights Reserved. The Havok
  688. * Logo, and the Havok buzzsaw logo are trademarks of Havok.  Title, ownership
  689. * rights, and intellectual property rights in the Havok software remain in
  690. * Havok and/or its suppliers.
  691. * 
  692. * Use of this software for evaluation purposes is subject to and indicates
  693. * acceptance of the End User licence Agreement for this product. A copy of
  694. * the license is included with this software and is also available at www.havok.com/tryhavok.
  695. * 
  696. */