extern/bullet/LinearMath/SimdVector3.h

   1 /*
   2 Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
   3
   4 This software is provided 'as-is', without any express or implied warranty.
   5 In no event will the authors be held liable for any damages arising from the use of this software.
   6 Permission is granted to anyone to use this software for any purpose,
   7 including commercial applications, and to alter it and redistribute it freely,
   8 subject to the following restrictions:
   9
  10 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
  11 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  12 3. This notice may not be removed or altered from any source distribution.
  13 */
  14
  15
  16
  17 #ifndef SIMD__VECTOR3_H
  18 #define SIMD__VECTOR3_H
  19
  20 #include "SimdQuadWord.h"
  21
  22
  23 ///SimdVector3 is 16byte aligned, and has an extra unused component m_w
  24 ///this extra component can be used by derived classes (Quaternion?) or by user
  25 class SimdVector3 : public SimdQuadWord {
  26
  27
  28 public:
  29         SIMD_FORCE_INLINE SimdVector3() {}
  30
  31
  32
  33         SIMD_FORCE_INLINE SimdVector3(const SimdScalar& x, const SimdScalar& y, const SimdScalar& z)
  34                 :SimdQuadWord(x,y,z,0.f)
  35         {
  36         }
  37
  38 //      SIMD_FORCE_INLINE SimdVector3(const SimdScalar& x, const SimdScalar& y, const SimdScalar& z,const SimdScalar& w)
  39 //              : SimdQuadWord(x,y,z,w)
  40 //      {
  41 //      }
  42
  43
  44
  45         SIMD_FORCE_INLINE SimdVector3& operator+=(const SimdVector3& v)
  46         {
  47                 m_x += v.x(); m_y += v.y(); m_z += v.z();
  48                 return *this;
  49         }
  50
  51
  52
  53         SIMD_FORCE_INLINE SimdVector3& operator-=(const SimdVector3& v)
  54         {
  55                 m_x -= v.x(); m_y -= v.y(); m_z -= v.z();
  56                 return *this;
  57         }
  58
  59         SIMD_FORCE_INLINE SimdVector3& operator*=(const SimdScalar& s)
  60         {
  61                 m_x *= s; m_y *= s; m_z *= s;
  62                 return *this;
  63         }
  64
  65         SIMD_FORCE_INLINE SimdVector3& operator/=(const SimdScalar& s)
  66         {
  67                 assert(s != SimdScalar(0.0));
  68                 return *this *= SimdScalar(1.0) / s;
  69         }
  70
  71         SIMD_FORCE_INLINE SimdScalar dot(const SimdVector3& v) const
  72         {
  73                 return m_x * v.x() + m_y * v.y() + m_z * v.z();
  74         }
  75
  76         SIMD_FORCE_INLINE SimdScalar length2() const
  77         {
  78                 return dot(*this);
  79         }
  80
  81         SIMD_FORCE_INLINE SimdScalar length() const
  82         {
  83                 return SimdSqrt(length2());
  84         }
  85
  86         SIMD_FORCE_INLINE SimdScalar distance2(const SimdVector3& v) const;
  87
  88         SIMD_FORCE_INLINE SimdScalar distance(const SimdVector3& v) const;
  89
  90         SIMD_FORCE_INLINE SimdVector3& normalize()
  91         {
  92                 return *this /= length();
  93         }
  94
  95         SIMD_FORCE_INLINE SimdVector3 normalized() const;
  96
  97         SIMD_FORCE_INLINE SimdVector3 rotate( const SimdVector3& wAxis, const SimdScalar angle );
  98
  99         SIMD_FORCE_INLINE SimdScalar angle(const SimdVector3& v) const
 100         {
 101                 SimdScalar s = SimdSqrt(length2() * v.length2());
 102                 assert(s != SimdScalar(0.0));
 103                 return SimdAcos(dot(v) / s);
 104         }
 105
 106         SIMD_FORCE_INLINE SimdVector3 absolute() const
 107         {
 108                 return SimdVector3(
 109                         SimdFabs(m_x),
 110                         SimdFabs(m_y),
 111                         SimdFabs(m_z));
 112         }
 113
 114         SIMD_FORCE_INLINE SimdVector3 cross(const SimdVector3& v) const
 115         {
 116                 return SimdVector3(
 117                         m_y * v.z() - m_z * v.y(),
 118                         m_z * v.x() - m_x * v.z(),
 119                         m_x * v.y() - m_y * v.x());
 120         }
 121
 122         SIMD_FORCE_INLINE SimdScalar triple(const SimdVector3& v1, const SimdVector3& v2) const
 123         {
 124                 return m_x * (v1.y() * v2.z() - v1.z() * v2.y()) +
 125                         m_y * (v1.z() * v2.x() - v1.x() * v2.z()) +
 126                         m_z * (v1.x() * v2.y() - v1.y() * v2.x());
 127         }
 128
 129         SIMD_FORCE_INLINE int minAxis() const
 130         {
 131                 return m_x < m_y ? (m_x < m_z ? 0 : 2) : (m_y < m_z ? 1 : 2);
 132         }
 133
 134         SIMD_FORCE_INLINE int maxAxis() const
 135         {
 136                 return m_x < m_y ? (m_y < m_z ? 2 : 1) : (m_x < m_z ? 2 : 0);
 137         }
 138
 139         SIMD_FORCE_INLINE int furthestAxis() const
 140         {
 141                 return absolute().minAxis();
 142         }
 143
 144         SIMD_FORCE_INLINE int closestAxis() const
 145         {
 146                 return absolute().maxAxis();
 147         }
 148
 149         SIMD_FORCE_INLINE void setInterpolate3(const SimdVector3& v0, const SimdVector3& v1, SimdScalar rt)
 150         {
 151                 SimdScalar s = 1.0f - rt;
 152                 m_x = s * v0[0] + rt * v1.x();
 153                 m_y = s * v0[1] + rt * v1.y();
 154                 m_z = s * v0[2] + rt * v1.z();
 155                 //don't do the unused w component
 156                 //              m_co[3] = s * v0[3] + rt * v1[3];
 157         }
 158
 159         SIMD_FORCE_INLINE SimdVector3 lerp(const SimdVector3& v, const SimdScalar& t) const
 160         {
 161                 return SimdVector3(m_x + (v.x() - m_x) * t,
 162                         m_y + (v.y() - m_y) * t,
 163                         m_z + (v.z() - m_z) * t);
 164         }
 165
 166
 167         SIMD_FORCE_INLINE SimdVector3& operator*=(const SimdVector3& v)
 168         {
 169                 m_x *= v.x(); m_y *= v.y(); m_z *= v.z();
 170                 return *this;
 171         }
 172
 173
 174
 175 };
 176
 177 SIMD_FORCE_INLINE SimdVector3
 178 operator+(const SimdVector3& v1, const SimdVector3& v2)
 179 {
 180         return SimdVector3(v1.x() + v2.x(), v1.y() + v2.y(), v1.z() + v2.z());
 181 }
 182
 183 SIMD_FORCE_INLINE SimdVector3
 184 operator*(const SimdVector3& v1, const SimdVector3& v2)
 185 {
 186         return SimdVector3(v1.x() * v2.x(), v1.y() * v2.y(), v1.z() *+ v2.z());
 187 }
 188
 189 SIMD_FORCE_INLINE SimdVector3
 190 operator-(const SimdVector3& v1, const SimdVector3& v2)
 191 {
 192         return SimdVector3(v1.x() - v2.x(), v1.y() - v2.y(), v1.z() - v2.z());
 193 }
 194
 195 SIMD_FORCE_INLINE SimdVector3
 196 operator-(const SimdVector3& v)
 197 {
 198         return SimdVector3(-v.x(), -v.y(), -v.z());
 199 }
 200
 201 SIMD_FORCE_INLINE SimdVector3
 202 operator*(const SimdVector3& v, const SimdScalar& s)
 203 {
 204         return SimdVector3(v.x() * s, v.y() * s, v.z() * s);
 205 }
 206
 207 SIMD_FORCE_INLINE SimdVector3
 208 operator*(const SimdScalar& s, const SimdVector3& v)
 209 {
 210         return v * s;
 211 }
 212
 213 SIMD_FORCE_INLINE SimdVector3
 214 operator/(const SimdVector3& v, const SimdScalar& s)
 215 {
 216         assert(s != SimdScalar(0.0));
 217         return v * (SimdScalar(1.0) / s);
 218 }
 219
 220 SIMD_FORCE_INLINE SimdVector3
 221 operator/(const SimdVector3& v1, const SimdVector3& v2)
 222 {
 223         return SimdVector3(v1.x() / v2.x(),v1.y() / v2.y(),v1.z() / v2.z());
 224 }
 225
 226 SIMD_FORCE_INLINE SimdScalar
 227 dot(const SimdVector3& v1, const SimdVector3& v2)
 228 {
 229         return v1.dot(v2);
 230 }
 231
 232
 233
 234 SIMD_FORCE_INLINE SimdScalar
 235 distance2(const SimdVector3& v1, const SimdVector3& v2)
 236 {
 237         return v1.distance2(v2);
 238 }
 239
 240
 241 SIMD_FORCE_INLINE SimdScalar
 242 distance(const SimdVector3& v1, const SimdVector3& v2)
 243 {
 244         return v1.distance(v2);
 245 }
 246
 247 SIMD_FORCE_INLINE SimdScalar
 248 angle(const SimdVector3& v1, const SimdVector3& v2)
 249 {
 250         return v1.angle(v2);
 251 }
 252
 253 SIMD_FORCE_INLINE SimdVector3
 254 cross(const SimdVector3& v1, const SimdVector3& v2)
 255 {
 256         return v1.cross(v2);
 257 }
 258
 259 SIMD_FORCE_INLINE SimdScalar
 260 triple(const SimdVector3& v1, const SimdVector3& v2, const SimdVector3& v3)
 261 {
 262         return v1.triple(v2, v3);
 263 }
 264
 265 SIMD_FORCE_INLINE SimdVector3
 266 lerp(const SimdVector3& v1, const SimdVector3& v2, const SimdScalar& t)
 267 {
 268         return v1.lerp(v2, t);
 269 }
 270
 271
 272 SIMD_FORCE_INLINE bool operator==(const SimdVector3& p1, const SimdVector3& p2)
 273 {
 274         return p1[0] == p2[0] && p1[1] == p2[1] && p1[2] == p2[2];
 275 }
 276
 277 SIMD_FORCE_INLINE SimdScalar SimdVector3::distance2(const SimdVector3& v) const
 278 {
 279         return (v - *this).length2();
 280 }
 281
 282 SIMD_FORCE_INLINE SimdScalar SimdVector3::distance(const SimdVector3& v) const
 283 {
 284         return (v - *this).length();
 285 }
 286
 287 SIMD_FORCE_INLINE SimdVector3 SimdVector3::normalized() const
 288 {
 289         return *this / length();
 290 }
 291
 292 SIMD_FORCE_INLINE SimdVector3 SimdVector3::rotate( const SimdVector3& wAxis, const SimdScalar angle )
 293 {
 294         // wAxis must be a unit lenght vector
 295
 296         SimdVector3 o = wAxis * wAxis.dot( *this );
 297         SimdVector3 x = *this - o;
 298         SimdVector3 y;
 299
 300         y = wAxis.cross( *this );
 301
 302         return ( o + x * SimdCos( angle ) + y * SimdSin( angle ) );
 303 }
 304
 305 class SimdVector4 : public SimdVector3
 306 {
 307 public:
 308
 309         SIMD_FORCE_INLINE SimdVector4() {}
 310
 311
 312         SIMD_FORCE_INLINE SimdVector4(const SimdScalar& x, const SimdScalar& y, const SimdScalar& z,const SimdScalar& w)
 313                 : SimdVector3(x,y,z)
 314         {
 315                 m_unusedW = w;
 316         }
 317
 318
 319         SIMD_FORCE_INLINE SimdVector4 absolute4() const
 320         {
 321                 return SimdVector4(
 322                         SimdFabs(m_x),
 323                         SimdFabs(m_y),
 324                         SimdFabs(m_z),
 325                         SimdFabs(m_unusedW));
 326         }
 327
 328
 329
 330         float   getW() const { return m_unusedW;}
 331
 332
 333                 SIMD_FORCE_INLINE int maxAxis4() const
 334         {
 335                 int maxIndex = -1;
 336                 float maxVal = -1e30f;
 337                 if (m_x > maxVal)
 338                 {
 339                         maxIndex = 0;
 340                         maxVal = m_x;
 341                 }
 342                 if (m_y > maxVal)
 343                 {
 344                         maxIndex = 1;
 345                         maxVal = m_y;
 346                 }
 347                 if (m_z > maxVal)
 348                 {
 349                         maxIndex = 2;
 350                         maxVal = m_z;
 351                 }
 352                 if (m_unusedW > maxVal)
 353                 {
 354                         maxIndex = 3;
 355                         maxVal = m_unusedW;
 356                 }
 357
 358
 359
 360
 361                 return maxIndex;
 362
 363         }
 364
 365
 366         SIMD_FORCE_INLINE int minAxis4() const
 367         {
 368                 int minIndex = -1;
 369                 float minVal = 1e30f;
 370                 if (m_x < minVal)
 371                 {
 372                         minIndex = 0;
 373                         minVal = m_x;
 374                 }
 375                 if (m_y < minVal)
 376                 {
 377                         minIndex = 1;
 378                         minVal = m_y;
 379                 }
 380                 if (m_z < minVal)
 381                 {
 382                         minIndex = 2;
 383                         minVal = m_z;
 384                 }
 385                 if (m_unusedW < minVal)
 386                 {
 387                         minIndex = 3;
 388                         minVal = m_unusedW;
 389                 }
 390
 391                 return minIndex;
 392
 393         }
 394
 395
 396         SIMD_FORCE_INLINE int closestAxis4() const
 397         {
 398                 return absolute4().maxAxis4();
 399         }
 400
 401 };
 402
 403 #endif //SIMD__VECTOR3_H