$search
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2011, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of Willow Garage, Inc. nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 */ 00034 00037 #ifndef COLLISION_CHECKING_VEC_3F_H 00038 #define COLLISION_CHECKING_VEC_3F_H 00039 00040 #include "collision_checking/BVH_defs.h" 00041 #include "collision_checking/roughly_comp.h" 00042 #include <cmath> 00043 #include <cstdlib> 00044 #include <algorithm> 00045 00047 namespace collision_checking 00048 { 00049 00050 00051 #if COLLISION_USE_SSE 00052 #include <xmmintrin.h> 00053 #include <pmmintrin.h> 00054 inline __m128 _mm_cross_ps(__m128 a , __m128 b) 00055 { 00056 // set to a[1][2][0][3] , b[2][0][1][3] 00057 // multiply 00058 __m128 xa = _mm_mul_ps( 00059 _mm_shuffle_ps(a, a, _MM_SHUFFLE(3, 0, 2, 1)), 00060 _mm_shuffle_ps(b, b, _MM_SHUFFLE(3, 1, 0, 2))); 00061 00062 // set to a[2][0][1][3] , b[1][2][0][3] 00063 // multiply 00064 __m128 xb = _mm_mul_ps( 00065 _mm_shuffle_ps(a, a, _MM_SHUFFLE(3, 1, 0, 2)), 00066 _mm_shuffle_ps(b, b, _MM_SHUFFLE(3, 0, 2, 1 ))); 00067 00068 // subtract 00069 return _mm_sub_ps(xa, xb); 00070 } 00071 00072 const __m128 xmms_0 = {0.f, 0.f, 0.f, 0.f}; 00073 00074 union ieee754_QNAN 00075 { 00076 const float f; 00077 struct 00078 { 00079 const unsigned int mantissa:23, exp:8, sign:1; 00080 }; 00081 00082 ieee754_QNAN() : f(0.0f), mantissa(0x7FFFFF), exp(0xFF), sign(0x0) {} 00083 } __attribute__ ((aligned (16))); 00084 00085 00086 class Vec3f 00087 { 00088 public: 00090 union {float v_[4]; __m128 v4; } __attribute__ ((aligned (16))); 00091 00092 Vec3f() { v4 = _mm_set1_ps(0); } 00093 00094 Vec3f(const Vec3f& other) 00095 { 00096 v4 = other.v4; 00097 } 00098 00099 Vec3f(const float* v) 00100 { 00101 v_[0] = v[0]; 00102 v_[1] = v[1]; 00103 v_[2] = v[2]; 00104 v_[3] = 0.0f; 00105 } 00106 00107 Vec3f(float x, float y, float z) 00108 { 00109 v_[0] = x; 00110 v_[1] = y; 00111 v_[2] = z; 00112 v_[3] = 0.0f; 00113 } 00114 00115 00116 Vec3f(__m128 v) 00117 { 00118 v4 = v; 00119 } 00120 00122 inline float operator [] (size_t i) const 00123 { 00124 return v_[i]; 00125 } 00126 00127 inline float& operator[] (size_t i) 00128 { 00129 return v_[i]; 00130 } 00131 00133 inline Vec3f& operator += (const Vec3f& other) 00134 { 00135 v4 = _mm_add_ps(v4, other.v4); 00136 return *this; 00137 } 00138 00139 00141 inline Vec3f& operator -= (const Vec3f& other) 00142 { 00143 v4 = _mm_sub_ps(v4, other.v4); 00144 return *this; 00145 } 00146 00148 inline void negate() 00149 { 00150 v4 = _mm_sub_ps(xmms_0, v4); 00151 } 00152 00154 inline Vec3f operator - () const 00155 { 00156 return Vec3f(_mm_sub_ps(xmms_0, v4)); 00157 } 00158 00159 00161 inline Vec3f operator + (const Vec3f& other) const 00162 { 00163 return Vec3f(_mm_add_ps(v4, other.v4)); 00164 } 00165 00167 inline Vec3f operator - (const Vec3f& other) const 00168 { 00169 return Vec3f(_mm_sub_ps(v4, other.v4)); 00170 } 00171 00173 inline Vec3f operator * (float t) const 00174 { 00175 return Vec3f(_mm_mul_ps(_mm_set1_ps(t), v4)); 00176 } 00177 00179 inline Vec3f cross(const Vec3f& other) const 00180 { 00181 return Vec3f(_mm_cross_ps(v4, other.v4)); 00182 } 00183 00185 inline float dot(const Vec3f& other) const 00186 { 00187 float d; 00188 register __m128 t = _mm_mul_ps(other.v4, v4); 00189 t = _mm_hadd_ps(t, t); 00190 t = _mm_hadd_ps(t, t); 00191 _mm_store_ss(&d, t); 00192 return d; 00193 } 00194 00196 inline bool normalize() 00197 { 00198 float sqr_length = sqrLength(); 00199 if(!isSmall(sqr_length, EPSILON)) 00200 { 00201 float inv_length = 1.0 / sqrt(sqr_length); 00202 v4 = _mm_mul_ps(_mm_set1_ps(inv_length), v4); 00203 return true; 00204 } 00205 return false; 00206 } 00207 00209 inline float length() const 00210 { 00211 return sqrt(sqrLength()); 00212 } 00213 00215 inline float sqrLength() const 00216 { 00217 float d; 00218 register __m128 t = _mm_mul_ps(v4, v4); 00219 t = _mm_hadd_ps(t, t); 00220 t = _mm_hadd_ps(t, t); 00221 _mm_store_ss(&d, t); 00222 return d; 00223 } 00224 00226 inline Vec3f& setValue(float x, float y, float z) 00227 { 00228 v_[0] = x; v_[1] = y; v_[2] = z; v_[3] = 0.0f; 00229 return *this; 00230 } 00231 00233 inline bool equal(const Vec3f& other) const 00234 { 00235 return roughlyEqual(v_[0], other.v_[0], EPSILON) && roughlyEqual(v_[1], other.v_[1], EPSILON) && roughlyEqual(v_[2], other.v_[2], EPSILON); 00236 } 00237 00238 private: 00240 static const float EPSILON; 00241 00242 }; 00243 00244 00246 inline Vec3f min(const Vec3f& a, const Vec3f& b) 00247 { 00248 Vec3f ret(_mm_min_ps(a.v4, b.v4)); 00249 return ret; 00250 } 00251 00253 inline Vec3f max(const Vec3f& a, const Vec3f& b) 00254 { 00255 Vec3f ret(_mm_max_ps(a.v4, b.v4)); 00256 return ret; 00257 } 00258 00259 inline Vec3f abs(const Vec3f& v) 00260 { 00261 const ieee754_QNAN mask; 00262 __m128 abs4mask = _mm_load1_ps(&mask.f); 00263 return Vec3f(_mm_and_ps(abs4mask, v.v4)); 00264 } 00265 #else 00266 00267 class Vec3f 00268 { 00269 public: 00271 BVH_REAL v_[3]; 00272 00273 Vec3f() { v_[0] = 0; v_[1] = 0; v_[2] = 0; } 00274 00275 Vec3f(const Vec3f& other) 00276 { 00277 v_[0] = other.v_[0]; 00278 v_[1] = other.v_[1]; 00279 v_[2] = other.v_[2]; 00280 } 00281 00282 Vec3f(const BVH_REAL* v) 00283 { 00284 v_[0] = v[0]; 00285 v_[1] = v[1]; 00286 v_[2] = v[2]; 00287 } 00288 00289 Vec3f(BVH_REAL x, BVH_REAL y, BVH_REAL z) 00290 { 00291 v_[0] = x; 00292 v_[1] = y; 00293 v_[2] = z; 00294 } 00295 00297 BVH_REAL operator [] (size_t i) const 00298 { 00299 return v_[i]; 00300 } 00301 00302 BVH_REAL& operator[] (size_t i) 00303 { 00304 return v_[i]; 00305 } 00306 00308 Vec3f& operator += (const Vec3f& other) 00309 { 00310 v_[0] += other.v_[0]; 00311 v_[1] += other.v_[1]; 00312 v_[2] += other.v_[2]; 00313 return *this; 00314 } 00315 00316 00318 Vec3f& operator -= (const Vec3f& other) 00319 { 00320 v_[0] -= other.v_[0]; 00321 v_[1] -= other.v_[1]; 00322 v_[2] -= other.v_[2]; 00323 return *this; 00324 } 00325 00327 void negate() 00328 { 00329 v_[0] = - v_[0]; 00330 v_[1] = - v_[1]; 00331 v_[2] = - v_[2]; 00332 } 00333 00335 Vec3f operator - () const 00336 { 00337 return Vec3f(-v_[0], -v_[1], -v_[2]); 00338 } 00339 00340 00342 Vec3f operator + (const Vec3f& other) const 00343 { 00344 return Vec3f(v_[0] + other.v_[0], v_[1] + other.v_[1], v_[2] + other.v_[2]); 00345 } 00346 00348 Vec3f operator - (const Vec3f& other) const 00349 { 00350 return Vec3f(v_[0] - other.v_[0], v_[1] - other.v_[1], v_[2] - other.v_[2]); 00351 } 00352 00354 Vec3f operator * (BVH_REAL t) const 00355 { 00356 return Vec3f(v_[0] * t, v_[1] * t, v_[2] * t); 00357 } 00358 00360 Vec3f cross(const Vec3f& other) const 00361 { 00362 return Vec3f(v_[1] * other.v_[2] - v_[2] * other.v_[1], 00363 v_[2] * other.v_[0] - v_[0] * other.v_[2], 00364 v_[0] * other.v_[1] - v_[1] * other.v_[0]); 00365 } 00366 00368 BVH_REAL dot(const Vec3f& other) const 00369 { 00370 return v_[0] * other.v_[0] + v_[1] * other.v_[1] + v_[2] * other.v_[2]; 00371 } 00372 00374 bool normalize() 00375 { 00376 BVH_REAL sqr_length = v_[0] * v_[0] + v_[1] * v_[1] + v_[2] * v_[2]; 00377 if(!isSmall(sqr_length, EPSILON * EPSILON)) 00378 { 00379 BVH_REAL inv_length = (BVH_REAL)1.0 / (BVH_REAL)sqrt(sqr_length); 00380 v_[0] *= inv_length; 00381 v_[1] *= inv_length; 00382 v_[2] *= inv_length; 00383 return true; 00384 } 00385 return false; 00386 } 00387 00389 BVH_REAL length() const 00390 { 00391 return sqrt(v_[0] * v_[0] + v_[1] * v_[1] + v_[2] * v_[2]); 00392 } 00393 00395 BVH_REAL sqrLength() const 00396 { 00397 return v_[0] * v_[0] + v_[1] * v_[1] + v_[2] * v_[2]; 00398 } 00399 00401 Vec3f& setValue(BVH_REAL x, BVH_REAL y, BVH_REAL z) 00402 { 00403 v_[0] = x; v_[1] = y; v_[2] = z; 00404 return *this; 00405 } 00406 00408 bool equal(const Vec3f& other) const 00409 { 00410 return roughlyEqual(v_[0], other.v_[0], EPSILON) && roughlyEqual(v_[1], other.v_[1], EPSILON) && roughlyEqual(v_[2], other.v_[2], EPSILON); 00411 } 00412 00413 private: 00415 static const BVH_REAL EPSILON; 00416 }; 00417 00418 00420 inline Vec3f min(const Vec3f& a, const Vec3f& b) 00421 { 00422 Vec3f ret(std::min(a[0], b[0]), std::min(a[1], b[1]), std::min(a[2], b[2])); 00423 return ret; 00424 } 00425 00427 inline Vec3f max(const Vec3f& a, const Vec3f& b) 00428 { 00429 Vec3f ret(std::max(a[0], b[0]), std::max(a[1], b[1]), std::max(a[2], b[2])); 00430 return ret; 00431 } 00432 00433 inline Vec3f abs(const Vec3f& v) 00434 { 00435 BVH_REAL x = v[0] < 0 ? -v[0] : v[0]; 00436 BVH_REAL y = v[1] < 0 ? -v[1] : v[1]; 00437 BVH_REAL z = v[2] < 0 ? -v[2] : v[2]; 00438 00439 return Vec3f(x, y, z); 00440 } 00441 #endif 00442 00443 Vec3f MxV(const Vec3f M[3], const Vec3f& v); 00444 Vec3f MTxV(const Vec3f M[3], const Vec3f& v); 00445 00446 } 00447 00448 #endif