00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #ifndef TF_VECTOR3_H
00018 #define TF_VECTOR3_H
00019
00020
00021 #include "Scalar.h"
00022 #include "MinMax.h"
00023
00024 namespace tf{
00025
00026 #define Vector3Data Vector3DoubleData
00027 #define Vector3DataName "Vector3DoubleData"
00028
00029
00030
00031
00038 class Vector3
00039 {
00040 public:
00041
00042 #if defined (__SPU__) && defined (__CELLOS_LV2__)
00043 tfScalar m_floats[4];
00044 public:
00045 TFSIMD_FORCE_INLINE const vec_float4& get128() const
00046 {
00047 return *((const vec_float4*)&m_floats[0]);
00048 }
00049 public:
00050 #else //__CELLOS_LV2__ __SPU__
00051 #ifdef TF_USE_SSE // _WIN32
00052 union {
00053 __m128 mVec128;
00054 tfScalar m_floats[4];
00055 };
00056 TFSIMD_FORCE_INLINE __m128 get128() const
00057 {
00058 return mVec128;
00059 }
00060 TFSIMD_FORCE_INLINE void set128(__m128 v128)
00061 {
00062 mVec128 = v128;
00063 }
00064 #else
00065 tfScalar m_floats[4];
00066 #endif
00067 #endif //__CELLOS_LV2__ __SPU__
00068
00069 public:
00070
00072 TFSIMD_FORCE_INLINE Vector3() {}
00073
00074
00080 TFSIMD_FORCE_INLINE Vector3(const tfScalar& x, const tfScalar& y, const tfScalar& z)
00081 {
00082 m_floats[0] = x;
00083 m_floats[1] = y;
00084 m_floats[2] = z;
00085 m_floats[3] = tfScalar(0.);
00086 }
00087
00090 TFSIMD_FORCE_INLINE Vector3& operator+=(const Vector3& v)
00091 {
00092
00093 m_floats[0] += v.m_floats[0]; m_floats[1] += v.m_floats[1];m_floats[2] += v.m_floats[2];
00094 return *this;
00095 }
00096
00097
00100 TFSIMD_FORCE_INLINE Vector3& operator-=(const Vector3& v)
00101 {
00102 m_floats[0] -= v.m_floats[0]; m_floats[1] -= v.m_floats[1];m_floats[2] -= v.m_floats[2];
00103 return *this;
00104 }
00107 TFSIMD_FORCE_INLINE Vector3& operator*=(const tfScalar& s)
00108 {
00109 m_floats[0] *= s; m_floats[1] *= s;m_floats[2] *= s;
00110 return *this;
00111 }
00112
00115 TFSIMD_FORCE_INLINE Vector3& operator/=(const tfScalar& s)
00116 {
00117 tfFullAssert(s != tfScalar(0.0));
00118 return *this *= tfScalar(1.0) / s;
00119 }
00120
00123 TFSIMD_FORCE_INLINE tfScalar dot(const Vector3& v) const
00124 {
00125 return m_floats[0] * v.m_floats[0] + m_floats[1] * v.m_floats[1] +m_floats[2] * v.m_floats[2];
00126 }
00127
00129 TFSIMD_FORCE_INLINE tfScalar length2() const
00130 {
00131 return dot(*this);
00132 }
00133
00135 TFSIMD_FORCE_INLINE tfScalar length() const
00136 {
00137 return tfSqrt(length2());
00138 }
00139
00142 TFSIMD_FORCE_INLINE tfScalar distance2(const Vector3& v) const;
00143
00146 TFSIMD_FORCE_INLINE tfScalar distance(const Vector3& v) const;
00147
00150 TFSIMD_FORCE_INLINE Vector3& normalize()
00151 {
00152 return *this /= length();
00153 }
00154
00156 TFSIMD_FORCE_INLINE Vector3 normalized() const;
00157
00161 TFSIMD_FORCE_INLINE Vector3 rotate( const Vector3& wAxis, const tfScalar angle ) const;
00162
00165 TFSIMD_FORCE_INLINE tfScalar angle(const Vector3& v) const
00166 {
00167 tfScalar s = tfSqrt(length2() * v.length2());
00168 tfFullAssert(s != tfScalar(0.0));
00169 return tfAcos(dot(v) / s);
00170 }
00172 TFSIMD_FORCE_INLINE Vector3 absolute() const
00173 {
00174 return Vector3(
00175 tfFabs(m_floats[0]),
00176 tfFabs(m_floats[1]),
00177 tfFabs(m_floats[2]));
00178 }
00181 TFSIMD_FORCE_INLINE Vector3 cross(const Vector3& v) const
00182 {
00183 return Vector3(
00184 m_floats[1] * v.m_floats[2] -m_floats[2] * v.m_floats[1],
00185 m_floats[2] * v.m_floats[0] - m_floats[0] * v.m_floats[2],
00186 m_floats[0] * v.m_floats[1] - m_floats[1] * v.m_floats[0]);
00187 }
00188
00189 TFSIMD_FORCE_INLINE tfScalar triple(const Vector3& v1, const Vector3& v2) const
00190 {
00191 return m_floats[0] * (v1.m_floats[1] * v2.m_floats[2] - v1.m_floats[2] * v2.m_floats[1]) +
00192 m_floats[1] * (v1.m_floats[2] * v2.m_floats[0] - v1.m_floats[0] * v2.m_floats[2]) +
00193 m_floats[2] * (v1.m_floats[0] * v2.m_floats[1] - v1.m_floats[1] * v2.m_floats[0]);
00194 }
00195
00198 TFSIMD_FORCE_INLINE int minAxis() const
00199 {
00200 return m_floats[0] < m_floats[1] ? (m_floats[0] <m_floats[2] ? 0 : 2) : (m_floats[1] <m_floats[2] ? 1 : 2);
00201 }
00202
00205 TFSIMD_FORCE_INLINE int maxAxis() const
00206 {
00207 return m_floats[0] < m_floats[1] ? (m_floats[1] <m_floats[2] ? 2 : 1) : (m_floats[0] <m_floats[2] ? 2 : 0);
00208 }
00209
00210 TFSIMD_FORCE_INLINE int furthestAxis() const
00211 {
00212 return absolute().minAxis();
00213 }
00214
00215 TFSIMD_FORCE_INLINE int closestAxis() const
00216 {
00217 return absolute().maxAxis();
00218 }
00219
00220 TFSIMD_FORCE_INLINE void setInterpolate3(const Vector3& v0, const Vector3& v1, tfScalar rt)
00221 {
00222 tfScalar s = tfScalar(1.0) - rt;
00223 m_floats[0] = s * v0.m_floats[0] + rt * v1.m_floats[0];
00224 m_floats[1] = s * v0.m_floats[1] + rt * v1.m_floats[1];
00225 m_floats[2] = s * v0.m_floats[2] + rt * v1.m_floats[2];
00226
00227
00228 }
00229
00233 TFSIMD_FORCE_INLINE Vector3 lerp(const Vector3& v, const tfScalar& t) const
00234 {
00235 return Vector3(m_floats[0] + (v.m_floats[0] - m_floats[0]) * t,
00236 m_floats[1] + (v.m_floats[1] - m_floats[1]) * t,
00237 m_floats[2] + (v.m_floats[2] -m_floats[2]) * t);
00238 }
00239
00242 TFSIMD_FORCE_INLINE Vector3& operator*=(const Vector3& v)
00243 {
00244 m_floats[0] *= v.m_floats[0]; m_floats[1] *= v.m_floats[1];m_floats[2] *= v.m_floats[2];
00245 return *this;
00246 }
00247
00249 TFSIMD_FORCE_INLINE const tfScalar& getX() const { return m_floats[0]; }
00251 TFSIMD_FORCE_INLINE const tfScalar& getY() const { return m_floats[1]; }
00253 TFSIMD_FORCE_INLINE const tfScalar& getZ() const { return m_floats[2]; }
00255 TFSIMD_FORCE_INLINE void setX(tfScalar x) { m_floats[0] = x;};
00257 TFSIMD_FORCE_INLINE void setY(tfScalar y) { m_floats[1] = y;};
00259 TFSIMD_FORCE_INLINE void setZ(tfScalar z) {m_floats[2] = z;};
00261 TFSIMD_FORCE_INLINE void setW(tfScalar w) { m_floats[3] = w;};
00263 TFSIMD_FORCE_INLINE const tfScalar& x() const { return m_floats[0]; }
00265 TFSIMD_FORCE_INLINE const tfScalar& y() const { return m_floats[1]; }
00267 TFSIMD_FORCE_INLINE const tfScalar& z() const { return m_floats[2]; }
00269 TFSIMD_FORCE_INLINE const tfScalar& w() const { return m_floats[3]; }
00270
00271
00272
00274 TFSIMD_FORCE_INLINE operator tfScalar *() { return &m_floats[0]; }
00275 TFSIMD_FORCE_INLINE operator const tfScalar *() const { return &m_floats[0]; }
00276
00277 TFSIMD_FORCE_INLINE bool operator==(const Vector3& other) const
00278 {
00279 return ((m_floats[3]==other.m_floats[3]) && (m_floats[2]==other.m_floats[2]) && (m_floats[1]==other.m_floats[1]) && (m_floats[0]==other.m_floats[0]));
00280 }
00281
00282 TFSIMD_FORCE_INLINE bool operator!=(const Vector3& other) const
00283 {
00284 return !(*this == other);
00285 }
00286
00290 TFSIMD_FORCE_INLINE void setMax(const Vector3& other)
00291 {
00292 tfSetMax(m_floats[0], other.m_floats[0]);
00293 tfSetMax(m_floats[1], other.m_floats[1]);
00294 tfSetMax(m_floats[2], other.m_floats[2]);
00295 tfSetMax(m_floats[3], other.w());
00296 }
00300 TFSIMD_FORCE_INLINE void setMin(const Vector3& other)
00301 {
00302 tfSetMin(m_floats[0], other.m_floats[0]);
00303 tfSetMin(m_floats[1], other.m_floats[1]);
00304 tfSetMin(m_floats[2], other.m_floats[2]);
00305 tfSetMin(m_floats[3], other.w());
00306 }
00307
00308 TFSIMD_FORCE_INLINE void setValue(const tfScalar& x, const tfScalar& y, const tfScalar& z)
00309 {
00310 m_floats[0]=x;
00311 m_floats[1]=y;
00312 m_floats[2]=z;
00313 m_floats[3] = tfScalar(0.);
00314 }
00315
00316 void getSkewSymmetricMatrix(Vector3* v0,Vector3* v1,Vector3* v2) const
00317 {
00318 v0->setValue(0. ,-z() ,y());
00319 v1->setValue(z() ,0. ,-x());
00320 v2->setValue(-y() ,x() ,0.);
00321 }
00322
00323 void setZero()
00324 {
00325 setValue(tfScalar(0.),tfScalar(0.),tfScalar(0.));
00326 }
00327
00328 TFSIMD_FORCE_INLINE bool isZero() const
00329 {
00330 return m_floats[0] == tfScalar(0) && m_floats[1] == tfScalar(0) && m_floats[2] == tfScalar(0);
00331 }
00332
00333 TFSIMD_FORCE_INLINE bool fuzzyZero() const
00334 {
00335 return length2() < TFSIMD_EPSILON;
00336 }
00337
00338 TFSIMD_FORCE_INLINE void serialize(struct Vector3Data& dataOut) const;
00339
00340 TFSIMD_FORCE_INLINE void deSerialize(const struct Vector3Data& dataIn);
00341
00342 TFSIMD_FORCE_INLINE void serializeFloat(struct Vector3FloatData& dataOut) const;
00343
00344 TFSIMD_FORCE_INLINE void deSerializeFloat(const struct Vector3FloatData& dataIn);
00345
00346 TFSIMD_FORCE_INLINE void serializeDouble(struct Vector3DoubleData& dataOut) const;
00347
00348 TFSIMD_FORCE_INLINE void deSerializeDouble(const struct Vector3DoubleData& dataIn);
00349
00350 } __attribute__ ((aligned(16)));
00351
00353 TFSIMD_FORCE_INLINE Vector3
00354 operator+(const Vector3& v1, const Vector3& v2)
00355 {
00356 return Vector3(v1.m_floats[0] + v2.m_floats[0], v1.m_floats[1] + v2.m_floats[1], v1.m_floats[2] + v2.m_floats[2]);
00357 }
00358
00360 TFSIMD_FORCE_INLINE Vector3
00361 operator*(const Vector3& v1, const Vector3& v2)
00362 {
00363 return Vector3(v1.m_floats[0] * v2.m_floats[0], v1.m_floats[1] * v2.m_floats[1], v1.m_floats[2] * v2.m_floats[2]);
00364 }
00365
00367 TFSIMD_FORCE_INLINE Vector3
00368 operator-(const Vector3& v1, const Vector3& v2)
00369 {
00370 return Vector3(v1.m_floats[0] - v2.m_floats[0], v1.m_floats[1] - v2.m_floats[1], v1.m_floats[2] - v2.m_floats[2]);
00371 }
00373 TFSIMD_FORCE_INLINE Vector3
00374 operator-(const Vector3& v)
00375 {
00376 return Vector3(-v.m_floats[0], -v.m_floats[1], -v.m_floats[2]);
00377 }
00378
00380 TFSIMD_FORCE_INLINE Vector3
00381 operator*(const Vector3& v, const tfScalar& s)
00382 {
00383 return Vector3(v.m_floats[0] * s, v.m_floats[1] * s, v.m_floats[2] * s);
00384 }
00385
00387 TFSIMD_FORCE_INLINE Vector3
00388 operator*(const tfScalar& s, const Vector3& v)
00389 {
00390 return v * s;
00391 }
00392
00394 TFSIMD_FORCE_INLINE Vector3
00395 operator/(const Vector3& v, const tfScalar& s)
00396 {
00397 tfFullAssert(s != tfScalar(0.0));
00398 return v * (tfScalar(1.0) / s);
00399 }
00400
00402 TFSIMD_FORCE_INLINE Vector3
00403 operator/(const Vector3& v1, const Vector3& v2)
00404 {
00405 return Vector3(v1.m_floats[0] / v2.m_floats[0],v1.m_floats[1] / v2.m_floats[1],v1.m_floats[2] / v2.m_floats[2]);
00406 }
00407
00409 TFSIMD_FORCE_INLINE tfScalar
00410 tfDot(const Vector3& v1, const Vector3& v2)
00411 {
00412 return v1.dot(v2);
00413 }
00414
00415
00417 TFSIMD_FORCE_INLINE tfScalar
00418 tfDistance2(const Vector3& v1, const Vector3& v2)
00419 {
00420 return v1.distance2(v2);
00421 }
00422
00423
00425 TFSIMD_FORCE_INLINE tfScalar
00426 tfDistance(const Vector3& v1, const Vector3& v2)
00427 {
00428 return v1.distance(v2);
00429 }
00430
00432 TFSIMD_FORCE_INLINE tfScalar
00433 tfAngle(const Vector3& v1, const Vector3& v2)
00434 {
00435 return v1.angle(v2);
00436 }
00437
00439 TFSIMD_FORCE_INLINE Vector3
00440 tfCross(const Vector3& v1, const Vector3& v2)
00441 {
00442 return v1.cross(v2);
00443 }
00444
00445 TFSIMD_FORCE_INLINE tfScalar
00446 tfTriple(const Vector3& v1, const Vector3& v2, const Vector3& v3)
00447 {
00448 return v1.triple(v2, v3);
00449 }
00450
00455 TFSIMD_FORCE_INLINE Vector3
00456 lerp(const Vector3& v1, const Vector3& v2, const tfScalar& t)
00457 {
00458 return v1.lerp(v2, t);
00459 }
00460
00461
00462
00463 TFSIMD_FORCE_INLINE tfScalar Vector3::distance2(const Vector3& v) const
00464 {
00465 return (v - *this).length2();
00466 }
00467
00468 TFSIMD_FORCE_INLINE tfScalar Vector3::distance(const Vector3& v) const
00469 {
00470 return (v - *this).length();
00471 }
00472
00473 TFSIMD_FORCE_INLINE Vector3 Vector3::normalized() const
00474 {
00475 return *this / length();
00476 }
00477
00478 TFSIMD_FORCE_INLINE Vector3 Vector3::rotate( const Vector3& wAxis, const tfScalar angle ) const
00479 {
00480
00481
00482 Vector3 o = wAxis * wAxis.dot( *this );
00483 Vector3 x = *this - o;
00484 Vector3 y;
00485
00486 y = wAxis.cross( *this );
00487
00488 return ( o + x * tfCos( angle ) + y * tfSin( angle ) );
00489 }
00490
00491 class tfVector4 : public Vector3
00492 {
00493 public:
00494
00495 TFSIMD_FORCE_INLINE tfVector4() {}
00496
00497
00498 TFSIMD_FORCE_INLINE tfVector4(const tfScalar& x, const tfScalar& y, const tfScalar& z,const tfScalar& w)
00499 : Vector3(x,y,z)
00500 {
00501 m_floats[3] = w;
00502 }
00503
00504
00505 TFSIMD_FORCE_INLINE tfVector4 absolute4() const
00506 {
00507 return tfVector4(
00508 tfFabs(m_floats[0]),
00509 tfFabs(m_floats[1]),
00510 tfFabs(m_floats[2]),
00511 tfFabs(m_floats[3]));
00512 }
00513
00514
00515
00516 tfScalar getW() const { return m_floats[3];}
00517
00518
00519 TFSIMD_FORCE_INLINE int maxAxis4() const
00520 {
00521 int maxIndex = -1;
00522 tfScalar maxVal = tfScalar(-TF_LARGE_FLOAT);
00523 if (m_floats[0] > maxVal)
00524 {
00525 maxIndex = 0;
00526 maxVal = m_floats[0];
00527 }
00528 if (m_floats[1] > maxVal)
00529 {
00530 maxIndex = 1;
00531 maxVal = m_floats[1];
00532 }
00533 if (m_floats[2] > maxVal)
00534 {
00535 maxIndex = 2;
00536 maxVal =m_floats[2];
00537 }
00538 if (m_floats[3] > maxVal)
00539 {
00540 maxIndex = 3;
00541 maxVal = m_floats[3];
00542 }
00543
00544
00545
00546
00547 return maxIndex;
00548
00549 }
00550
00551
00552 TFSIMD_FORCE_INLINE int minAxis4() const
00553 {
00554 int minIndex = -1;
00555 tfScalar minVal = tfScalar(TF_LARGE_FLOAT);
00556 if (m_floats[0] < minVal)
00557 {
00558 minIndex = 0;
00559 minVal = m_floats[0];
00560 }
00561 if (m_floats[1] < minVal)
00562 {
00563 minIndex = 1;
00564 minVal = m_floats[1];
00565 }
00566 if (m_floats[2] < minVal)
00567 {
00568 minIndex = 2;
00569 minVal =m_floats[2];
00570 }
00571 if (m_floats[3] < minVal)
00572 {
00573 minIndex = 3;
00574 minVal = m_floats[3];
00575 }
00576
00577 return minIndex;
00578
00579 }
00580
00581
00582 TFSIMD_FORCE_INLINE int closestAxis4() const
00583 {
00584 return absolute4().maxAxis4();
00585 }
00586
00587
00588
00589
00597
00598
00599
00600
00601
00602
00603
00610 TFSIMD_FORCE_INLINE void setValue(const tfScalar& x, const tfScalar& y, const tfScalar& z,const tfScalar& w)
00611 {
00612 m_floats[0]=x;
00613 m_floats[1]=y;
00614 m_floats[2]=z;
00615 m_floats[3]=w;
00616 }
00617
00618
00619 };
00620
00621
00623 TFSIMD_FORCE_INLINE void tfSwapScalarEndian(const tfScalar& sourceVal, tfScalar& destVal)
00624 {
00625 unsigned char* dest = (unsigned char*) &destVal;
00626 unsigned char* src = (unsigned char*) &sourceVal;
00627 dest[0] = src[7];
00628 dest[1] = src[6];
00629 dest[2] = src[5];
00630 dest[3] = src[4];
00631 dest[4] = src[3];
00632 dest[5] = src[2];
00633 dest[6] = src[1];
00634 dest[7] = src[0];
00635 }
00637 TFSIMD_FORCE_INLINE void tfSwapVector3Endian(const Vector3& sourceVec, Vector3& destVec)
00638 {
00639 for (int i=0;i<4;i++)
00640 {
00641 tfSwapScalarEndian(sourceVec[i],destVec[i]);
00642 }
00643
00644 }
00645
00647 TFSIMD_FORCE_INLINE void tfUnSwapVector3Endian(Vector3& vector)
00648 {
00649
00650 Vector3 swappedVec;
00651 for (int i=0;i<4;i++)
00652 {
00653 tfSwapScalarEndian(vector[i],swappedVec[i]);
00654 }
00655 vector = swappedVec;
00656 }
00657
00658 TFSIMD_FORCE_INLINE void tfPlaneSpace1 (const Vector3& n, Vector3& p, Vector3& q)
00659 {
00660 if (tfFabs(n.z()) > TFSIMDSQRT12) {
00661
00662 tfScalar a = n[1]*n[1] + n[2]*n[2];
00663 tfScalar k = tfRecipSqrt (a);
00664 p.setValue(0,-n[2]*k,n[1]*k);
00665
00666 q.setValue(a*k,-n[0]*p[2],n[0]*p[1]);
00667 }
00668 else {
00669
00670 tfScalar a = n.x()*n.x() + n.y()*n.y();
00671 tfScalar k = tfRecipSqrt (a);
00672 p.setValue(-n.y()*k,n.x()*k,0);
00673
00674 q.setValue(-n.z()*p.y(),n.z()*p.x(),a*k);
00675 }
00676 }
00677
00678
00679 struct Vector3FloatData
00680 {
00681 float m_floats[4];
00682 };
00683
00684 struct Vector3DoubleData
00685 {
00686 double m_floats[4];
00687
00688 };
00689
00690 TFSIMD_FORCE_INLINE void Vector3::serializeFloat(struct Vector3FloatData& dataOut) const
00691 {
00693 for (int i=0;i<4;i++)
00694 dataOut.m_floats[i] = float(m_floats[i]);
00695 }
00696
00697 TFSIMD_FORCE_INLINE void Vector3::deSerializeFloat(const struct Vector3FloatData& dataIn)
00698 {
00699 for (int i=0;i<4;i++)
00700 m_floats[i] = tfScalar(dataIn.m_floats[i]);
00701 }
00702
00703
00704 TFSIMD_FORCE_INLINE void Vector3::serializeDouble(struct Vector3DoubleData& dataOut) const
00705 {
00707 for (int i=0;i<4;i++)
00708 dataOut.m_floats[i] = double(m_floats[i]);
00709 }
00710
00711 TFSIMD_FORCE_INLINE void Vector3::deSerializeDouble(const struct Vector3DoubleData& dataIn)
00712 {
00713 for (int i=0;i<4;i++)
00714 m_floats[i] = tfScalar(dataIn.m_floats[i]);
00715 }
00716
00717
00718 TFSIMD_FORCE_INLINE void Vector3::serialize(struct Vector3Data& dataOut) const
00719 {
00721 for (int i=0;i<4;i++)
00722 dataOut.m_floats[i] = m_floats[i];
00723 }
00724
00725 TFSIMD_FORCE_INLINE void Vector3::deSerialize(const struct Vector3Data& dataIn)
00726 {
00727 for (int i=0;i<4;i++)
00728 m_floats[i] = dataIn.m_floats[i];
00729 }
00730
00731 }
00732
00733 #endif //TFSIMD__VECTOR3_H