diff --git a/trunk/unmanaged/ubODE-OpenSim/OPCODE/Ice/IceFPU.h b/trunk/unmanaged/ubODE-OpenSim/OPCODE/Ice/IceFPU.h index 487ac529..cd2bfb99 100644 --- a/trunk/unmanaged/ubODE-OpenSim/OPCODE/Ice/IceFPU.h +++ b/trunk/unmanaged/ubODE-OpenSim/OPCODE/Ice/IceFPU.h @@ -217,12 +217,6 @@ #define FCMOVB_ST7 _asm _emit 0xda _asm _emit 0xc7 #define FCMOVNB_ST7 _asm _emit 0xdb _asm _emit 0xc7 - //! A global function to find MAX(a,b) using FCOMI/FCMOV - inline_ float FCMax2(float a, float b) - { - return (a > b) ? a : b; - } - //! A global function to find MIN(a,b) using FCOMI/FCMOV inline_ float FCMin2(float a, float b) { @@ -241,6 +235,55 @@ return (a < b) ? ((a < c) ? a : c) : ((b < c) ? b : c); } + inline_ void MinMax(float& min, float& max, const float a, const float b, const float c) + { + if (a > b) + { + if (b > c) + { + max = a; + min = c; + } + else + { + min = b; + max = (a > c) ? a : c; + } + } + else + { + if (c > b) + { + max = c; + min = a; + } + else + { + max = b; + min = (a < c) ? a : c; + } + } + } + + inline bool inExtent(const float a, const float b, const float c, const float e) + { + if (a > b) + { + if (((b < c) ? b : c) > e) + return false; + if (((a > c) ? a : c) < -e) + return false; + } + else + { + if(((a < c) ? a : c) > e) + return false; + if(((b > c) ? b : c) < -e) + return false; + } + return true; + } + inline_ int ConvertToSortable(float f) { int Fi = SIR(f); diff --git a/trunk/unmanaged/ubODE-OpenSim/OPCODE/Ice/IcePoint.h b/trunk/unmanaged/ubODE-OpenSim/OPCODE/Ice/IcePoint.h index b0429f50..5fb5b5e4 100644 --- a/trunk/unmanaged/ubODE-OpenSim/OPCODE/Ice/IcePoint.h +++ b/trunk/unmanaged/ubODE-OpenSim/OPCODE/Ice/IcePoint.h @@ -14,6 +14,14 @@ #if defined(__AVX__) #include + +inline_ void iceStore3f(float *res, __m128 ma) +{ + __m128 mb; + _mm_store_sd((double *)res, _mm_castps_pd(ma)); + mb = _mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(ma), 0x02)); + _mm_store_ss(res + 2, mb); +} #endif // Forward declarations @@ -68,17 +76,13 @@ #if defined(__AVX__) inline_ Point& Add(const Point& p) { - float restmp[4]; __m128 ma, mb; ma = _mm_loadu_ps(&x); mb = _mm_loadu_ps(p); ma = _mm_add_ps(ma, mb); - _mm_storeu_ps(restmp, ma); - x = restmp[0]; - y = restmp[1]; - z = restmp[2]; + iceStore3f(&x, ma); return *this; } #else @@ -90,17 +94,13 @@ #if defined(__AVX__) inline_ Point& Add(const float f[3]) { - float restmp[4]; __m128 ma, mb; ma = _mm_loadu_ps(&x); mb = _mm_loadu_ps(f); ma = _mm_add_ps(ma, mb); - _mm_storeu_ps(restmp, ma); - x = restmp[0]; - y = restmp[1]; - z = restmp[2]; + iceStore3f(&x, ma); return *this; } #else @@ -110,17 +110,13 @@ #if defined(__AVX__) inline_ Point& Add(const Point& p, const Point& q) { - float restmp[4]; __m128 ma, mb; ma = _mm_loadu_ps(p); mb = _mm_loadu_ps(q); ma = _mm_add_ps(ma, mb); - _mm_storeu_ps(restmp, ma); - x = restmp[0]; - y = restmp[1]; - z = restmp[2]; + iceStore3f(&x, ma); return *this; } #else @@ -131,17 +127,13 @@ #if defined(__AVX__) inline_ Point& Sub(const Point& p) { - float restmp[4]; __m128 ma, mb; ma = _mm_loadu_ps(&x); mb = _mm_loadu_ps(p); ma = _mm_sub_ps(ma, mb); - _mm_storeu_ps(restmp, ma); - x = restmp[0]; - y = restmp[1]; - z = restmp[2]; + iceStore3f(&x, ma); return *this; } #else @@ -154,17 +146,13 @@ #if defined(__AVX__) inline_ Point& Sub(const float f[3]) { - float restmp[4]; __m128 ma, mb; ma = _mm_loadu_ps(&x); mb = _mm_loadu_ps(f); ma = _mm_sub_ps(ma, mb); - _mm_storeu_ps(restmp, ma); - x = restmp[0]; - y = restmp[1]; - z = restmp[2]; + iceStore3f(&x, ma); return *this; } #else @@ -175,17 +163,13 @@ #if defined(__AVX__) inline_ Point& Sub(const Point& p, const Point& q) { - float restmp[4]; __m128 ma, mb; ma = _mm_loadu_ps(p); mb = _mm_loadu_ps(q); ma = _mm_sub_ps(ma, mb); - _mm_storeu_ps(restmp, ma); - x = restmp[0]; - y = restmp[1]; - z = restmp[2]; + iceStore3f(&x, ma); return *this; } #else @@ -200,17 +184,13 @@ #if defined(__AVX__) inline_ Point& Mult(float s) { - float restmp[4]; __m128 ma, mc; ma = _mm_loadu_ps(&x); mc = _mm_set1_ps(s); ma = _mm_mul_ps(ma, mc); - _mm_storeu_ps(restmp, ma); - x = restmp[0]; - y = restmp[1]; - z = restmp[2]; + iceStore3f(&x, ma); return *this; } #else @@ -220,17 +200,13 @@ #if defined(__AVX__) inline_ Point& Mult(const Point& a, float scalar) { - float restmp[4]; __m128 ma, mc; ma = _mm_loadu_ps(a); mc = _mm_set1_ps(scalar); ma = _mm_mul_ps(ma, mc); - _mm_storeu_ps(restmp, ma); - x = restmp[0]; - y = restmp[1]; - z = restmp[2]; + iceStore3f(&x, ma); return *this; } #else @@ -246,7 +222,6 @@ #if defined(__AVX__) inline_ Point& Mac(const Point& a, const Point& b, float scalar) { - float restmp[4]; __m128 ma, mb, mc; mb = _mm_loadu_ps(b); @@ -256,10 +231,7 @@ ma = _mm_loadu_ps(a); ma = _mm_add_ps(ma, mb); - _mm_storeu_ps(restmp, ma); - x = restmp[0]; - y = restmp[1]; - z = restmp[2]; + iceStore3f(&x, ma); return *this; } #else @@ -275,7 +247,6 @@ #if defined(__AVX__) inline_ Point& Mac(const Point& a, float scalar) { - float restmp[4]; __m128 ma, mb, mc; mb = _mm_loadu_ps(a); @@ -285,10 +256,7 @@ ma = _mm_loadu_ps(&x); ma = _mm_add_ps(ma, mb); - _mm_storeu_ps(restmp, ma); - x = restmp[0]; - y = restmp[1]; - z = restmp[2]; + iceStore3f(&x, ma); return *this; } #else @@ -304,7 +272,6 @@ #if defined(__AVX__) inline_ Point& Msc(const Point& a, const Point& b, float scalar) { - float restmp[4]; __m128 ma, mb, mc; mb = _mm_loadu_ps(b); @@ -314,10 +281,7 @@ ma = _mm_loadu_ps(a); ma = _mm_sub_ps(ma, mb); - _mm_storeu_ps(restmp, ma); - x = restmp[0]; - y = restmp[1]; - z = restmp[2]; + iceStore3f(&x, ma); return *this; } #else @@ -333,7 +297,6 @@ #if defined(__AVX__) inline_ Point& Msc(const Point& a, float scalar) { - float restmp[4]; __m128 ma, mb, mc; mb = _mm_loadu_ps(a); @@ -343,10 +306,7 @@ ma = _mm_loadu_ps(&x); ma = _mm_sub_ps(ma, mb); - _mm_storeu_ps(restmp, ma); - x = restmp[0]; - y = restmp[1]; - z = restmp[2]; + iceStore3f(&x, ma); return *this; } #else @@ -362,7 +322,6 @@ #if defined(__AVX__) inline_ Point& Mac2(const Point& a, const Point& b, float scalarb, const Point& c, float scalarc) { - float restmp[4]; __m128 ma, mb, mc, ms; mb = _mm_loadu_ps(b); @@ -377,10 +336,7 @@ ma = _mm_loadu_ps(a); ma = _mm_add_ps(ma, mb); - _mm_storeu_ps(restmp, ma); - x = restmp[0]; - y = restmp[1]; - z = restmp[2]; + iceStore3f(&x, ma); return *this; } #else @@ -396,7 +352,6 @@ #if defined(__AVX__) inline_ Point& Msc2(const Point& a, const Point& b, float scalarb, const Point& c, float scalarc) { - float restmp[4]; __m128 ma, mb, mc, ms; mb = _mm_loadu_ps(b); @@ -411,10 +366,7 @@ ma = _mm_loadu_ps(a); ma = _mm_sub_ps(ma, mb); - _mm_storeu_ps(restmp, ma); - x = restmp[0]; - y = restmp[1]; - z = restmp[2]; + iceStore3f(&x, ma); return *this; } #else @@ -442,7 +394,6 @@ #if defined(__AVX__) inline_ Point& Lerp(const Point& a, const Point& b, float t) { - float restmp[4]; __m128 ma, mb, mt; ma = _mm_loadu_ps(a); @@ -453,10 +404,7 @@ mb = _mm_mul_ps(mb, mt); ma = _mm_add_ps(ma, mb); - _mm_storeu_ps(restmp, ma); - x = restmp[0]; - y = restmp[1]; - z = restmp[2]; + iceStore3f(&x, ma); return *this; } #else @@ -593,7 +541,6 @@ inline_ Point& Normalize() { __m128 ma, mc; - float restmp[4]; ma = _mm_loadu_ps(&x); mc = _mm_dp_ps(ma, ma, 0x71); @@ -604,10 +551,7 @@ mc = _mm_set1_ps(m); ma = _mm_mul_ps(ma, mc); - _mm_storeu_ps(restmp, ma); - x = restmp[0]; - y = restmp[1]; - z = restmp[2]; + iceStore3f(&x, ma); } return *this; } @@ -716,7 +660,6 @@ __m128 ma, mb, t1, t2, t3, t4; ma = _mm_loadu_ps(a); mb = _mm_loadu_ps(b); - float restmp[4]; t1 = _mm_shuffle_ps(ma, ma, _MM_SHUFFLE(3, 0, 2, 1)); t2 = _mm_shuffle_ps(mb, mb, _MM_SHUFFLE(3, 1, 0, 2)); @@ -728,10 +671,7 @@ t4 = _mm_mul_ps(t1, t2); ma = _mm_sub_ps(t3, t4); - _mm_storeu_ps(restmp, ma); - x = restmp[0]; - y = restmp[1]; - x = restmp[2]; + iceStore3f(&x, ma); } #else inline_ Point& Cross(const Point& a, const Point& b) @@ -1207,7 +1147,7 @@ #if defined(__AVX__) inline_ Point& operator*=(const Matrix3x3& mat) { - __m128 ma, t0, t1, t2, m0, m1, m2, m3; + __m128 ma, t0, t1, t2, m0, m1, m2; float xx, yy, zz; class ShadowMatrix3x3 { public: float m[3][3]; }; // To allow inlining @@ -1217,14 +1157,11 @@ t1 = _mm_loadu_ps(Mat->m[1]); t2 = _mm_loadu_ps(Mat->m[2]); - m0 = _mm_shuffle_ps(t0, t1, _MM_SHUFFLE(1, 0, 1, 0)); // x0 y0 x1 y1 - m2 = _mm_shuffle_ps(t0, t1, _MM_SHUFFLE(3, 2, 3, 2)); // z0 w0 z1 w1 - m1 = _mm_shuffle_ps(t1, t2, _MM_SHUFFLE(1, 0, 1, 0)); // x1 y1 x2 y2 - m3 = _mm_shuffle_ps(t1, t2, _MM_SHUFFLE(3, 2, 3, 2)); // z1 w1 z2 w2 - - t0 = _mm_shuffle_ps(m0, m1, _MM_SHUFFLE(2, 2, 2, 0)); //x0 x1 x2 x2 - t1 = _mm_shuffle_ps(m0, m1, _MM_SHUFFLE(3, 3, 3, 1)); //y0 y1 y2 y2 - t2 = _mm_shuffle_ps(m2, m3, _MM_SHUFFLE(2, 2, 2, 0)); //z0 z1 z2 z2 + m0 = _mm_movelh_ps(t0, t1); // x0 y0 x1 y1 + m2 = _mm_movehl_ps(t1, t0); // z0 w0 z1 w1 + t0 = _mm_shuffle_ps(m0, t2, _MM_SHUFFLE(3, 0, 2, 0)); //x0 x1 x2 w2 + t1 = _mm_shuffle_ps(m0, t2, _MM_SHUFFLE(3, 1, 3, 1)); //y0 y1 y2 w2 + t2 = _mm_shuffle_ps(m2, t2, _MM_SHUFFLE(3, 2, 2, 0)); //z0 z1 z2 w2 ma = _mm_loadu_ps(&x); m0 = _mm_dp_ps(ma, t0, 0x71); @@ -1256,30 +1193,29 @@ #if defined(__AVX__) inline_ Point& operator*=(const Matrix4x4& mat) { - __m128 ma, t0, t1, t2, m0, m1, m2, m3; + __m128 ma, t0, t1, t2, m0, m1, m2; float xx, yy, zz; class ShadowMatrix4x4 { public: float m[4][4]; }; // To allow inlining const ShadowMatrix4x4* Mat = (const ShadowMatrix4x4*)&mat; - t0 = _mm_loadu_ps(Mat->m[0]); - t1 = _mm_loadu_ps(Mat->m[1]); - t2 = _mm_loadu_ps(Mat->m[2]); + t0 = _mm_loadu_ps(Mat->m[0]); //x0 y0 z0 w0 + t1 = _mm_loadu_ps(Mat->m[1]); //x1 y1 z1 w1 + t2 = _mm_loadu_ps(Mat->m[2]); //x2 y2 z2 w2 - m0 = _mm_shuffle_ps(t0, t1, _MM_SHUFFLE(1, 0, 1, 0)); // x0 y0 x1 y1 - m2 = _mm_shuffle_ps(t0, t1, _MM_SHUFFLE(3, 2, 3, 2)); // z0 w0 z1 w1 - m1 = _mm_shuffle_ps(t1, t2, _MM_SHUFFLE(1, 0, 1, 0)); // x1 y1 x2 y2 - m3 = _mm_shuffle_ps(t1, t2, _MM_SHUFFLE(3, 2, 3, 2)); // z1 w1 z2 w2 - - t0 = _mm_shuffle_ps(m0, m1, _MM_SHUFFLE(2, 2, 2, 0)); //x0 x1 x2 x2 - t1 = _mm_shuffle_ps(m0, m1, _MM_SHUFFLE(3, 3, 3, 1)); //y0 y1 y2 y2 - t2 = _mm_shuffle_ps(m2, m3, _MM_SHUFFLE(2, 2, 2, 0)); //z0 z1 z2 z2 + m0 = _mm_movelh_ps(t0, t1); // x0 y0 x1 y1 + m2 = _mm_movehl_ps(t1, t0); // z0 w0 z1 w1 + t0 = _mm_shuffle_ps(m0, t2, _MM_SHUFFLE(3, 0, 2, 0)); //x0 x1 x2 w2 + t1 = _mm_shuffle_ps(m0, t2, _MM_SHUFFLE(3, 1, 3, 1)); //y0 y1 y2 w2 + t2 = _mm_shuffle_ps(m2, t2, _MM_SHUFFLE(3, 2, 2, 0)); //z0 z1 z2 w2 ma = _mm_loadu_ps(&x); m0 = _mm_dp_ps(ma, t0, 0x71); xx = _mm_cvtss_f32(m0) + Mat->m[3][0]; + m1 = _mm_dp_ps(ma, t1, 0x71); yy = _mm_cvtss_f32(m1) + Mat->m[3][1]; + m2 = _mm_dp_ps(ma, t2, 0x71); zz = _mm_cvtss_f32(m2) + Mat->m[3][2]; x = xx; y = yy; z = zz; diff --git a/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_AABBCollider.cpp b/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_AABBCollider.cpp index b1e5d23a..14ad078c 100644 --- a/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_AABBCollider.cpp +++ b/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_AABBCollider.cpp @@ -43,7 +43,7 @@ using namespace Opcode; //! AABB-triangle test #define AABB_PRIM(prim_index, flag) \ /* Request vertices from the app */ \ - VertexPointers VP; ConversionArea VC; mIMesh->GetTriangle(VP, prim_index, VC); \ + VertexPointers VP; mIMesh->GetTriangle(VP, prim_index); \ mLeafVerts[0] = *VP.Vertex[0]; \ mLeafVerts[1] = *VP.Vertex[1]; \ mLeafVerts[2] = *VP.Vertex[2]; \ @@ -93,52 +93,11 @@ bool AABBCollider::Collide(AABBCache& cache, const CollisionAABB& box, const Mod // Init collision query if(InitQuery(cache, box)) return true; - if(!model.HasLeafNodes()) - { - if(model.IsQuantized()) - { - const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree(); + const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree(); - // Setup dequantization coeffs - mCenterCoeff = Tree->mCenterCoeff; - mExtentsCoeff = Tree->mExtentsCoeff; - - // Perform collision query - if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes()); - else _Collide(Tree->GetNodes()); - } - else - { - const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree(); - - // Perform collision query - if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes()); - else _Collide(Tree->GetNodes()); - } - } - else - { - if(model.IsQuantized()) - { - const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree(); - - // Setup dequantization coeffs - mCenterCoeff = Tree->mCenterCoeff; - mExtentsCoeff = Tree->mExtentsCoeff; - - // Perform collision query - if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes()); - else _Collide(Tree->GetNodes()); - } - else - { - const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree(); - - // Perform collision query - if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes()); - else _Collide(Tree->GetNodes()); - } - } + // Perform collision query + if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes()); + else _Collide(Tree->GetNodes()); return true; } @@ -600,48 +559,10 @@ bool HybridAABBCollider::Collide(AABBCache& cache, const CollisionAABB& box, con mTouchedPrimitives = &mTouchedBoxes; // Now, do the actual query against leaf boxes - if(!model.HasLeafNodes()) - { - if(model.IsQuantized()) - { - const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree(); + const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree(); - // Setup dequantization coeffs - mCenterCoeff = Tree->mCenterCoeff; - mExtentsCoeff = Tree->mExtentsCoeff; - - // Perform collision query - we don't want primitive tests here! - _CollideNoPrimitiveTest(Tree->GetNodes()); - } - else - { - const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree(); - - // Perform collision query - we don't want primitive tests here! - _CollideNoPrimitiveTest(Tree->GetNodes()); - } - } - else - { - if(model.IsQuantized()) - { - const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree(); - - // Setup dequantization coeffs - mCenterCoeff = Tree->mCenterCoeff; - mExtentsCoeff = Tree->mExtentsCoeff; - - // Perform collision query - we don't want primitive tests here! - _CollideNoPrimitiveTest(Tree->GetNodes()); - } - else - { - const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree(); - - // Perform collision query - we don't want primitive tests here! - _CollideNoPrimitiveTest(Tree->GetNodes()); - } - } + // Perform collision query - we don't want primitive tests here! + _CollideNoPrimitiveTest(Tree->GetNodes()); // We only have a list of boxes so far if(GetContactStatus()) diff --git a/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_BaseModel.cpp b/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_BaseModel.cpp index 9520d9ef..acd85fa4 100644 --- a/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_BaseModel.cpp +++ b/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_BaseModel.cpp @@ -42,11 +42,6 @@ OPCODECREATE::OPCODECREATE() mIMesh = null; mSettings.mRules = SPLIT_SPLATTER_POINTS | SPLIT_GEOM_CENTER; mSettings.mLimit = 1; // Mandatory for complete trees - mNoLeaf = true; - mQuantized = true; -#ifdef __MESHMERIZER_H__ - mCollisionHull = false; -#endif // __MESHMERIZER_H__ mKeepOriginal = false; mCanRemap = false; } @@ -89,28 +84,11 @@ void BaseModel::ReleaseBase() * \return true if success */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -bool BaseModel::CreateTree(bool no_leaf, bool quantized) +bool BaseModel::CreateTree() { DELETESINGLE(mTree); - // Setup model code - if(no_leaf) mModelCode |= OPC_NO_LEAF; - else mModelCode &= ~OPC_NO_LEAF; - - if(quantized) mModelCode |= OPC_QUANTIZED; - else mModelCode &= ~OPC_QUANTIZED; - - // Create the correct class - if(mModelCode & OPC_NO_LEAF) - { - if(mModelCode & OPC_QUANTIZED) mTree = new AABBQuantizedNoLeafTree; - else mTree = new AABBNoLeafTree; - } - else - { - if(mModelCode & OPC_QUANTIZED) mTree = new AABBQuantizedTree; - else mTree = new AABBCollisionTree; - } + mTree = new AABBNoLeafTree; CHECKALLOC(mTree); return true; diff --git a/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_BaseModel.h b/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_BaseModel.h index c6072dbd..0251b064 100644 --- a/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_BaseModel.h +++ b/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_BaseModel.h @@ -28,11 +28,6 @@ MeshInterface* mIMesh; //!< Mesh interface (access to triangles & vertices) (*) BuildSettings mSettings; //!< Builder's settings - bool mNoLeaf; //!< true => discard leaf nodes (else use a normal tree) - bool mQuantized; //!< true => quantize the tree (else use a normal tree) -#ifdef __MESHMERIZER_H__ - bool mCollisionHull; //!< true => use convex hull + GJK -#endif // __MESHMERIZER_H__ bool mKeepOriginal; //!< true => keep a copy of the original tree (debug purpose) bool mCanRemap; //!< true => allows OPCODE to reorganize client arrays @@ -42,8 +37,6 @@ enum ModelFlag { - OPC_QUANTIZED = (1<<0), //!< Compressed/uncompressed tree - OPC_NO_LEAF = (1<<1), //!< Leaf/NoLeaf tree OPC_SINGLE_NODE = (1<<2) //!< Special case for 1-node models }; @@ -114,22 +107,6 @@ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ udword GetNbNodes() const { return mTree->GetNbNodes(); } - /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - /** - * Checks whether the tree has leaf nodes or not. - * \return true if the tree has leaf nodes (normal tree), else false (optimized tree) - */ - /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - inline_ BOOL HasLeafNodes() const { return !(mModelCode & OPC_NO_LEAF); } - - /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - /** - * Checks whether the tree is quantized or not. - * \return true if the tree is quantized - */ - /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - inline_ BOOL IsQuantized() const { return mModelCode & OPC_QUANTIZED; } - /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Checks whether the model has a single node or not. This special case must be handled separately. @@ -169,7 +146,7 @@ AABBOptimizedTree* mTree; //!< Optimized tree owned by the model // Internal methods void ReleaseBase(); - bool CreateTree(bool no_leaf, bool quantized); + bool CreateTree(); }; #endif //__OPC_BASEMODEL_H__ diff --git a/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_BoxBoxOverlap.h b/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_BoxBoxOverlap.h index 757a17dd..2487bc69 100644 --- a/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_BoxBoxOverlap.h +++ b/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_BoxBoxOverlap.h @@ -15,6 +15,7 @@ * \param cb [in] center from box B * \return true if boxes overlap */ + /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ BOOL AABBTreeCollider::BoxBoxOverlap(const Point& ea, const Point& ca, const Point& eb, const Point& cb) { @@ -26,40 +27,65 @@ inline_ BOOL AABBTreeCollider::BoxBoxOverlap(const Point& ea, const Point& ca, c // Class I : A's basis vectors float Tx = (mR1to0.m[0][0]*cb.x + mR1to0.m[1][0]*cb.y + mR1to0.m[2][0]*cb.z) + mT1to0.x - ca.x; t = ea.x + eb.x*mAR.m[0][0] + eb.y*mAR.m[1][0] + eb.z*mAR.m[2][0]; - if(GREATER(Tx, t)) return FALSE; + if (fabs(Tx) > t) return FALSE; float Ty = (mR1to0.m[0][1]*cb.x + mR1to0.m[1][1]*cb.y + mR1to0.m[2][1]*cb.z) + mT1to0.y - ca.y; t = ea.y + eb.x*mAR.m[0][1] + eb.y*mAR.m[1][1] + eb.z*mAR.m[2][1]; - if(GREATER(Ty, t)) return FALSE; + if (fabs(Ty) > t) return FALSE; float Tz = (mR1to0.m[0][2]*cb.x + mR1to0.m[1][2]*cb.y + mR1to0.m[2][2]*cb.z) + mT1to0.z - ca.z; t = ea.z + eb.x*mAR.m[0][2] + eb.y*mAR.m[1][2] + eb.z*mAR.m[2][2]; - if(GREATER(Tz, t)) return FALSE; + if (fabs(Tz) > t) return FALSE; // Class II : B's basis vectors t = Tx*mR1to0.m[0][0] + Ty*mR1to0.m[0][1] + Tz*mR1to0.m[0][2]; t2 = ea.x*mAR.m[0][0] + ea.y*mAR.m[0][1] + ea.z*mAR.m[0][2] + eb.x; - if(GREATER(t, t2)) return FALSE; + if (fabs(t) > t2) return FALSE; t = Tx*mR1to0.m[1][0] + Ty*mR1to0.m[1][1] + Tz*mR1to0.m[1][2]; t2 = ea.x*mAR.m[1][0] + ea.y*mAR.m[1][1] + ea.z*mAR.m[1][2] + eb.y; - if(GREATER(t, t2)) return FALSE; + if (fabs(t) > t2) return FALSE; t = Tx*mR1to0.m[2][0] + Ty*mR1to0.m[2][1] + Tz*mR1to0.m[2][2]; t2 = ea.x*mAR.m[2][0] + ea.y*mAR.m[2][1] + ea.z*mAR.m[2][2] + eb.z; - if(GREATER(t, t2)) return FALSE; + if (fabs(t) > t2) return FALSE; // Class III : 9 cross products // Cool trick: always perform the full test for first level, regardless of settings. // That way pathological cases (such as the pencils scene) are quickly rejected anyway ! if(mFullBoxBoxTest || mNbBVBVTests==1) { - t = Tz*mR1to0.m[0][1] - Ty*mR1to0.m[0][2]; t2 = ea.y*mAR.m[0][2] + ea.z*mAR.m[0][1] + eb.y*mAR.m[2][0] + eb.z*mAR.m[1][0]; if(GREATER(t, t2)) return FALSE; // L = A0 x B0 - t = Tz*mR1to0.m[1][1] - Ty*mR1to0.m[1][2]; t2 = ea.y*mAR.m[1][2] + ea.z*mAR.m[1][1] + eb.x*mAR.m[2][0] + eb.z*mAR.m[0][0]; if(GREATER(t, t2)) return FALSE; // L = A0 x B1 - t = Tz*mR1to0.m[2][1] - Ty*mR1to0.m[2][2]; t2 = ea.y*mAR.m[2][2] + ea.z*mAR.m[2][1] + eb.x*mAR.m[1][0] + eb.y*mAR.m[0][0]; if(GREATER(t, t2)) return FALSE; // L = A0 x B2 - t = Tx*mR1to0.m[0][2] - Tz*mR1to0.m[0][0]; t2 = ea.x*mAR.m[0][2] + ea.z*mAR.m[0][0] + eb.y*mAR.m[2][1] + eb.z*mAR.m[1][1]; if(GREATER(t, t2)) return FALSE; // L = A1 x B0 - t = Tx*mR1to0.m[1][2] - Tz*mR1to0.m[1][0]; t2 = ea.x*mAR.m[1][2] + ea.z*mAR.m[1][0] + eb.x*mAR.m[2][1] + eb.z*mAR.m[0][1]; if(GREATER(t, t2)) return FALSE; // L = A1 x B1 - t = Tx*mR1to0.m[2][2] - Tz*mR1to0.m[2][0]; t2 = ea.x*mAR.m[2][2] + ea.z*mAR.m[2][0] + eb.x*mAR.m[1][1] + eb.y*mAR.m[0][1]; if(GREATER(t, t2)) return FALSE; // L = A1 x B2 - t = Ty*mR1to0.m[0][0] - Tx*mR1to0.m[0][1]; t2 = ea.x*mAR.m[0][1] + ea.y*mAR.m[0][0] + eb.y*mAR.m[2][2] + eb.z*mAR.m[1][2]; if(GREATER(t, t2)) return FALSE; // L = A2 x B0 - t = Ty*mR1to0.m[1][0] - Tx*mR1to0.m[1][1]; t2 = ea.x*mAR.m[1][1] + ea.y*mAR.m[1][0] + eb.x*mAR.m[2][2] + eb.z*mAR.m[0][2]; if(GREATER(t, t2)) return FALSE; // L = A2 x B1 - t = Ty*mR1to0.m[2][0] - Tx*mR1to0.m[2][1]; t2 = ea.x*mAR.m[2][1] + ea.y*mAR.m[2][0] + eb.x*mAR.m[1][2] + eb.y*mAR.m[0][2]; if(GREATER(t, t2)) return FALSE; // L = A2 x B2 + t = Tz*mR1to0.m[0][1] - Ty*mR1to0.m[0][2]; + t2 = ea.y*mAR.m[0][2] + ea.z*mAR.m[0][1] + eb.y*mAR.m[2][0] + eb.z*mAR.m[1][0]; + if (fabs(t) > t2) return FALSE; // L = A0 x B0 + + t = Tz*mR1to0.m[1][1] - Ty*mR1to0.m[1][2]; + t2 = ea.y*mAR.m[1][2] + ea.z*mAR.m[1][1] + eb.x*mAR.m[2][0] + eb.z*mAR.m[0][0]; + if (fabs(t) > t2) return FALSE; // L = A0 x B1 + + t = Tz*mR1to0.m[2][1] - Ty*mR1to0.m[2][2]; + t2 = ea.y*mAR.m[2][2] + ea.z*mAR.m[2][1] + eb.x*mAR.m[1][0] + eb.y*mAR.m[0][0]; + if (fabs(t) > t2) return FALSE; // L = A0 x B2 + + t = Tx*mR1to0.m[0][2] - Tz*mR1to0.m[0][0]; + t2 = ea.x*mAR.m[0][2] + ea.z*mAR.m[0][0] + eb.y*mAR.m[2][1] + eb.z*mAR.m[1][1]; + if (fabs(t) > t2) return FALSE; // L = A1 x B0 + t = Tx*mR1to0.m[1][2] - Tz*mR1to0.m[1][0]; + t2 = ea.x*mAR.m[1][2] + ea.z*mAR.m[1][0] + eb.x*mAR.m[2][1] + eb.z*mAR.m[0][1]; + if (fabs(t) > t2) return FALSE; // L = A1 x B1 + + t = Tx*mR1to0.m[2][2] - Tz*mR1to0.m[2][0]; + t2 = ea.x*mAR.m[2][2] + ea.z*mAR.m[2][0] + eb.x*mAR.m[1][1] + eb.y*mAR.m[0][1]; + if (fabs(t) > t2) return FALSE; // L = A1 x B2 + + t = Ty*mR1to0.m[0][0] - Tx*mR1to0.m[0][1]; + t2 = ea.x*mAR.m[0][1] + ea.y*mAR.m[0][0] + eb.y*mAR.m[2][2] + eb.z*mAR.m[1][2]; + if (fabs(t) > t2) return FALSE; // L = A2 x B0 + + t = Ty*mR1to0.m[1][0] - Tx*mR1to0.m[1][1]; + t2 = ea.x*mAR.m[1][1] + ea.y*mAR.m[1][0] + eb.x*mAR.m[2][2] + eb.z*mAR.m[0][2]; + if (fabs(t) > t2) return FALSE; // L = A2 x B1 + + t = Ty*mR1to0.m[2][0] - Tx*mR1to0.m[2][1]; + t2 = ea.x*mAR.m[2][1] + ea.y*mAR.m[2][0] + eb.x*mAR.m[1][2] + eb.y*mAR.m[0][2]; + if (fabs(t) > t2) return FALSE; // L = A2 x B2 } return TRUE; } @@ -73,37 +99,71 @@ inline_ BOOL OBBCollider::BoxBoxOverlap(const Point& extents, const Point& cente float t,t2; // Class I : A's basis vectors - float Tx = mTBoxToModel.x - center.x; t = extents.x + mBBx1; if(GREATER(Tx, t)) return FALSE; - float Ty = mTBoxToModel.y - center.y; t = extents.y + mBBy1; if(GREATER(Ty, t)) return FALSE; - float Tz = mTBoxToModel.z - center.z; t = extents.z + mBBz1; if(GREATER(Tz, t)) return FALSE; + float Tx = mTBoxToModel.x - center.x; + t = extents.x + mBBx1; + if (fabs(Tx) > t) return FALSE; + + float Ty = mTBoxToModel.y - center.y; + t = extents.y + mBBy1; + if (fabs(Ty) > t) return FALSE; + + float Tz = mTBoxToModel.z - center.z; + t = extents.z + mBBz1; + if (fabs(Tz) > t) return FALSE; // Class II : B's basis vectors t = Tx*mRBoxToModel.m[0][0] + Ty*mRBoxToModel.m[0][1] + Tz*mRBoxToModel.m[0][2]; t2 = extents.x*mAR.m[0][0] + extents.y*mAR.m[0][1] + extents.z*mAR.m[0][2] + mBoxExtents.x; - if(GREATER(t, t2)) return FALSE; + if (fabs(t) > t2) return FALSE; t = Tx*mRBoxToModel.m[1][0] + Ty*mRBoxToModel.m[1][1] + Tz*mRBoxToModel.m[1][2]; t2 = extents.x*mAR.m[1][0] + extents.y*mAR.m[1][1] + extents.z*mAR.m[1][2] + mBoxExtents.y; - if(GREATER(t, t2)) return FALSE; + if (fabs(t) > t2) return FALSE; t = Tx*mRBoxToModel.m[2][0] + Ty*mRBoxToModel.m[2][1] + Tz*mRBoxToModel.m[2][2]; t2 = extents.x*mAR.m[2][0] + extents.y*mAR.m[2][1] + extents.z*mAR.m[2][2] + mBoxExtents.z; - if(GREATER(t, t2)) return FALSE; + if (fabs(t) > t2) return FALSE; // Class III : 9 cross products // Cool trick: always perform the full test for first level, regardless of settings. // That way pathological cases (such as the pencils scene) are quickly rejected anyway ! if(mFullBoxBoxTest || mNbVolumeBVTests==1) { - t = Tz*mRBoxToModel.m[0][1] - Ty*mRBoxToModel.m[0][2]; t2 = extents.y*mAR.m[0][2] + extents.z*mAR.m[0][1] + mBB_1; if(GREATER(t, t2)) return FALSE; // L = A0 x B0 - t = Tz*mRBoxToModel.m[1][1] - Ty*mRBoxToModel.m[1][2]; t2 = extents.y*mAR.m[1][2] + extents.z*mAR.m[1][1] + mBB_2; if(GREATER(t, t2)) return FALSE; // L = A0 x B1 - t = Tz*mRBoxToModel.m[2][1] - Ty*mRBoxToModel.m[2][2]; t2 = extents.y*mAR.m[2][2] + extents.z*mAR.m[2][1] + mBB_3; if(GREATER(t, t2)) return FALSE; // L = A0 x B2 - t = Tx*mRBoxToModel.m[0][2] - Tz*mRBoxToModel.m[0][0]; t2 = extents.x*mAR.m[0][2] + extents.z*mAR.m[0][0] + mBB_4; if(GREATER(t, t2)) return FALSE; // L = A1 x B0 - t = Tx*mRBoxToModel.m[1][2] - Tz*mRBoxToModel.m[1][0]; t2 = extents.x*mAR.m[1][2] + extents.z*mAR.m[1][0] + mBB_5; if(GREATER(t, t2)) return FALSE; // L = A1 x B1 - t = Tx*mRBoxToModel.m[2][2] - Tz*mRBoxToModel.m[2][0]; t2 = extents.x*mAR.m[2][2] + extents.z*mAR.m[2][0] + mBB_6; if(GREATER(t, t2)) return FALSE; // L = A1 x B2 - t = Ty*mRBoxToModel.m[0][0] - Tx*mRBoxToModel.m[0][1]; t2 = extents.x*mAR.m[0][1] + extents.y*mAR.m[0][0] + mBB_7; if(GREATER(t, t2)) return FALSE; // L = A2 x B0 - t = Ty*mRBoxToModel.m[1][0] - Tx*mRBoxToModel.m[1][1]; t2 = extents.x*mAR.m[1][1] + extents.y*mAR.m[1][0] + mBB_8; if(GREATER(t, t2)) return FALSE; // L = A2 x B1 - t = Ty*mRBoxToModel.m[2][0] - Tx*mRBoxToModel.m[2][1]; t2 = extents.x*mAR.m[2][1] + extents.y*mAR.m[2][0] + mBB_9; if(GREATER(t, t2)) return FALSE; // L = A2 x B2 + t = Tz*mRBoxToModel.m[0][1] - Ty*mRBoxToModel.m[0][2]; + t2 = extents.y*mAR.m[0][2] + extents.z*mAR.m[0][1] + mBB_1; + if (fabs(t) > t2) return FALSE; // L = A0 x B0 + + t = Tz*mRBoxToModel.m[1][1] - Ty*mRBoxToModel.m[1][2]; + t2 = extents.y*mAR.m[1][2] + extents.z*mAR.m[1][1] + mBB_2; + if (fabs(t) > t2) return FALSE; // L = A0 x B1 + + t = Tz*mRBoxToModel.m[2][1] - Ty*mRBoxToModel.m[2][2]; + t2 = extents.y*mAR.m[2][2] + extents.z*mAR.m[2][1] + mBB_3; + if (fabs(t) > t2) return FALSE; // L = A0 x B2 + + t = Tx*mRBoxToModel.m[0][2] - Tz*mRBoxToModel.m[0][0]; + t2 = extents.x*mAR.m[0][2] + extents.z*mAR.m[0][0] + mBB_4; + if (fabs(t) > t2) return FALSE; // L = A1 x B0 + + t = Tx*mRBoxToModel.m[1][2] - Tz*mRBoxToModel.m[1][0]; + t2 = extents.x*mAR.m[1][2] + extents.z*mAR.m[1][0] + mBB_5; + if (fabs(t) > t2) return FALSE; // L = A1 x B1 + + t = Tx*mRBoxToModel.m[2][2] - Tz*mRBoxToModel.m[2][0]; + t2 = extents.x*mAR.m[2][2] + extents.z*mAR.m[2][0] + mBB_6; + if (fabs(t) > t2) return FALSE; // L = A1 x B2 + + t = Ty*mRBoxToModel.m[0][0] - Tx*mRBoxToModel.m[0][1]; + t2 = extents.x*mAR.m[0][1] + extents.y*mAR.m[0][0] + mBB_7; + if (fabs(t) > t2) return FALSE; // L = A2 x B0 + + t = Ty*mRBoxToModel.m[1][0] - Tx*mRBoxToModel.m[1][1]; + t2 = extents.x*mAR.m[1][1] + extents.y*mAR.m[1][0] + mBB_8; + if (fabs(t) > t2) return FALSE; // L = A2 x B1 + + t = Ty*mRBoxToModel.m[2][0] - Tx*mRBoxToModel.m[2][1]; + t2 = extents.x*mAR.m[2][1] + extents.y*mAR.m[2][0] + mBB_9; + if (fabs(t) > t2) return FALSE; // L = A2 x B2 } return TRUE; } @@ -113,10 +173,35 @@ inline_ BOOL AABBCollider::AABBAABBOverlap(const Point& extents, const Point& ce { // Stats mNbVolumeBVTests++; +#if defined(__AVX__) + const __m128 sign = _mm_set1_ps(-0.0f); + __m128 ma, mb, mc; - float tx = mBox.mCenter.x - center.x; float ex = extents.x + mBox.mExtents.x; if(GREATER(tx, ex)) return FALSE; - float ty = mBox.mCenter.y - center.y; float ey = extents.y + mBox.mExtents.y; if(GREATER(ty, ey)) return FALSE; - float tz = mBox.mCenter.z - center.z; float ez = extents.z + mBox.mExtents.z; if(GREATER(tz, ez)) return FALSE; + ma = _mm_loadu_ps(&(mBox.mCenter.x)); + mb = _mm_loadu_ps(&(center.x)); + ma = _mm_sub_ps(ma, mb); - return TRUE; + ma = _mm_andnot_ps(sign, ma); + + mb = _mm_loadu_ps(&(mBox.mExtents.x)); + mc = _mm_loadu_ps(&(extents.x)); + mb = _mm_add_ps(mb, mc); + + ma = _mm_cmpgt_ps(ma, mb); + return ((_mm_movemask_ps(ma) & 0x07) == 0); + +#else + float tx = mBox.mCenter.x - center.x; + float ex = extents.x + mBox.mExtents.x; + if (fabs(tx) > ex) return FALSE; + + float ty = mBox.mCenter.y - center.y; + float ey = extents.y + mBox.mExtents.y; + if (fabs(ty) > ey) return FALSE; + + float tz = mBox.mCenter.z - center.z; + float ez = extents.z + mBox.mExtents.z; + if (fabs(tz) > ez) return FALSE; + return TRUE; +#endif } diff --git a/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_Common.h b/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_Common.h index f1349903..ee40b6f1 100644 --- a/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_Common.h +++ b/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_Common.h @@ -20,11 +20,25 @@ #ifndef __OPC_COMMON_H__ #define __OPC_COMMON_H__ -// [GOTTFRIED]: Just a small change for readability. -#ifdef OPC_CPU_COMPARE - #define GREATER(x, y) AIR(x) > IR(y) -#else - #define GREATER(x, y) fabsf(x) > (y) +#if defined(__AVX__) +ODE_PURE_INLINE bool avxBoxesOverlap(const dReal* acenter, const dReal* bcenter, const dReal* aext, const dReal* bext) +{ + const __m128 sign = _mm_set1_ps(-0.0f); + __m128 ma, mb, mc; + + ma = _mm_loadu_ps(acenter); + mb = _mm_loadu_ps(bcenter); + ma = _mm_sub_ps(ma, mb); + + ma = _mm_andnot_ps(sign, ma); + + mb = _mm_loadu_ps(aext); + mc = _mm_loadu_ps(bext); + mb = _mm_add_ps(mb, mc); + + ma = _mm_cmpgt_ps(ma, mb); + return ((_mm_movemask_ps(ma) & 0x07) == 0); +} #endif class OPCODE_API CollisionAABB @@ -65,6 +79,9 @@ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ BOOL IsInside(const CollisionAABB& box) const { +#if defined (__AVX__) + return avxBoxesOverlap(&(box.mCenter.x), &(mCenter.x), &(box.mExtents.x), &(mExtents.x)); +#else if(box.GetMin(0)>GetMin(0)) return FALSE; if(box.GetMin(1)>GetMin(1)) return FALSE; if(box.GetMin(2)>GetMin(2)) return FALSE; @@ -72,7 +89,8 @@ if(box.GetMax(1)Build(LeafTree)) goto FreeAndExit; @@ -311,24 +312,9 @@ udword HybridModel::GetUsedBytes() const inline_ void ComputeMinMax(Point& min, Point& max, const VertexPointers& vp) { - // Compute triangle's AABB = a leaf box -#ifdef OPC_USE_FCOMI // a 15% speedup on my machine, not much - min.x = FCMin3(vp.Vertex[0]->x, vp.Vertex[1]->x, vp.Vertex[2]->x); - max.x = FCMax3(vp.Vertex[0]->x, vp.Vertex[1]->x, vp.Vertex[2]->x); - - min.y = FCMin3(vp.Vertex[0]->y, vp.Vertex[1]->y, vp.Vertex[2]->y); - max.y = FCMax3(vp.Vertex[0]->y, vp.Vertex[1]->y, vp.Vertex[2]->y); - - min.z = FCMin3(vp.Vertex[0]->z, vp.Vertex[1]->z, vp.Vertex[2]->z); - max.z = FCMax3(vp.Vertex[0]->z, vp.Vertex[1]->z, vp.Vertex[2]->z); -#else - min = *vp.Vertex[0]; - max = *vp.Vertex[0]; - min.Min(*vp.Vertex[1]); - max.Max(*vp.Vertex[1]); - min.Min(*vp.Vertex[2]); - max.Max(*vp.Vertex[2]); -#endif + MinMax(min.x, max.x, vp.Vertex[0]->x, vp.Vertex[1]->x, vp.Vertex[2]->x); + MinMax(min.y, max.y, vp.Vertex[0]->y, vp.Vertex[1]->y, vp.Vertex[2]->y); + MinMax(min.z, max.z, vp.Vertex[0]->z, vp.Vertex[1]->z, vp.Vertex[2]->z); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -341,10 +327,11 @@ inline_ void ComputeMinMax(Point& min, Point& max, const VertexPointers& vp) /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool HybridModel::Refit() { + return false; +/* if(!mIMesh) return false; if(!mTree) return false; - if(IsQuantized()) return false; if(HasLeafNodes()) return false; const LeafTriangles* LT = GetLeafTriangles(); @@ -450,18 +437,15 @@ bool HybridModel::Refit() CurrentBox.GetMin(Min_); CurrentBox.GetMax(Max_); } -#ifdef OPC_USE_FCOMI + Min.x = FCMin2(Min.x, Min_.x); Max.x = FCMax2(Max.x, Max_.x); Min.y = FCMin2(Min.y, Min_.y); Max.y = FCMax2(Max.y, Max_.y); Min.z = FCMin2(Min.z, Min_.z); Max.z = FCMax2(Max.z, Max_.z); -#else - Min.Min(Min_); - Max.Max(Max_); -#endif Current.mAABB.SetMinMax(Min, Max); } return true; +*/ } diff --git a/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_LSSCollider.cpp b/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_LSSCollider.cpp index 557fbe4b..3767b419 100644 --- a/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_LSSCollider.cpp +++ b/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_LSSCollider.cpp @@ -43,7 +43,7 @@ using namespace Opcode; //! LSS-triangle overlap test #define LSS_PRIM(prim_index, flag) \ /* Request vertices from the app */ \ - VertexPointers VP; ConversionArea VC; mIMesh->GetTriangle(VP, prim_index, VC); \ + VertexPointers VP; mIMesh->GetTriangle(VP, prim_index); \ \ /* Perform LSS-tri overlap test */ \ if(LSSTriOverlap(*VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2])) \ @@ -95,52 +95,11 @@ bool LSSCollider::Collide(LSSCache& cache, const LSS& lss, const Model& model, c // Init collision query if(InitQuery(cache, lss, worldl, worldm)) return true; - if(!model.HasLeafNodes()) - { - if(model.IsQuantized()) - { - const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree(); + const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree(); - // Setup dequantization coeffs - mCenterCoeff = Tree->mCenterCoeff; - mExtentsCoeff = Tree->mExtentsCoeff; - - // Perform collision query - if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes()); - else _Collide(Tree->GetNodes()); - } - else - { - const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree(); - - // Perform collision query - if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes()); - else _Collide(Tree->GetNodes()); - } - } - else - { - if(model.IsQuantized()) - { - const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree(); - - // Setup dequantization coeffs - mCenterCoeff = Tree->mCenterCoeff; - mExtentsCoeff = Tree->mExtentsCoeff; - - // Perform collision query - if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes()); - else _Collide(Tree->GetNodes()); - } - else - { - const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree(); - - // Perform collision query - if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes()); - else _Collide(Tree->GetNodes()); - } - } + // Perform collision query + if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes()); + else _Collide(Tree->GetNodes()); return true; } @@ -628,49 +587,10 @@ bool HybridLSSCollider::Collide(LSSCache& cache, const LSS& lss, const HybridMod mTouchedBoxes.Reset(); mTouchedPrimitives = &mTouchedBoxes; - // Now, do the actual query against leaf boxes - if(!model.HasLeafNodes()) - { - if(model.IsQuantized()) - { - const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree(); + const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree(); - // Setup dequantization coeffs - mCenterCoeff = Tree->mCenterCoeff; - mExtentsCoeff = Tree->mExtentsCoeff; - - // Perform collision query - we don't want primitive tests here! - _CollideNoPrimitiveTest(Tree->GetNodes()); - } - else - { - const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree(); - - // Perform collision query - we don't want primitive tests here! - _CollideNoPrimitiveTest(Tree->GetNodes()); - } - } - else - { - if(model.IsQuantized()) - { - const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree(); - - // Setup dequantization coeffs - mCenterCoeff = Tree->mCenterCoeff; - mExtentsCoeff = Tree->mExtentsCoeff; - - // Perform collision query - we don't want primitive tests here! - _CollideNoPrimitiveTest(Tree->GetNodes()); - } - else - { - const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree(); - - // Perform collision query - we don't want primitive tests here! - _CollideNoPrimitiveTest(Tree->GetNodes()); - } - } + // Perform collision query - we don't want primitive tests here! + _CollideNoPrimitiveTest(Tree->GetNodes()); // We only have a list of boxes so far if(GetContactStatus()) diff --git a/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_MeshInterface.cpp b/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_MeshInterface.cpp index 157464e1..c09ef0cc 100644 --- a/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_MeshInterface.cpp +++ b/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_MeshInterface.cpp @@ -33,21 +33,7 @@ * This class is an interface between us and user-defined meshes. Meshes can be defined in a lot of ways, and here we * try to support most of them. * - * Basically you have two options: - * - callbacks, if OPC_USE_CALLBACKS is defined in OPC_Settings.h. - * - else pointers. - * - * If using pointers, you can also use strides or not. Strides are used when OPC_USE_STRIDE is defined. - * - * - * CALLBACKS: - * - * Using callbacks is the most generic way to feed OPCODE with your meshes. Indeed, you just have to give - * access to three vertices at the end of the day. It's up to you to fetch them from your database, using - * whatever method you want. Hence your meshes can lie in system memory or AGP, be indexed or not, use 16 - * or 32-bits indices, you can decompress them on-the-fly if needed, etc. On the other hand, a callback is - * called each time OPCODE needs access to a particular triangle, so there might be a slight overhead. - * + * To make things clear: geometry & topology are NOT stored in the collision system, * in order to save some ram. So, when the system needs them to perform accurate intersection * tests, you're requested to provide the triangle-vertices corresponding to a given face index. @@ -67,17 +53,8 @@ * triangle.Vertex[2] = MyMesh->GetVertex(Tri->mVRef[2]); * } * - * // Setup callbacks - * MeshInterface0->SetCallback(ColCallback, udword(Mesh0)); - * MeshInterface1->SetCallback(ColCallback, udword(Mesh1)); * \endcode * - * Of course, you should make this callback as fast as possible. And you're also not supposed - * to modify the geometry *after* the collision trees have been built. The alternative was to - * store the geometry & topology in the collision system as well (as in RAPID) but we have found - * this approach to waste a lot of ram in many cases. - * - * * POINTERS: * * If you're internally using the following canonical structures: @@ -128,21 +105,8 @@ using namespace Opcode; MeshInterface::MeshInterface() : mNbTris (0), mNbVerts (0), -#ifdef OPC_USE_CALLBACKS - mUserData (null), - mObjCallback (null), - mExUserData (null), - mObjExCallback (null), -#else - #ifdef OPC_USE_STRIDE - mTriStride (sizeof(IndexedTriangle)), - mVertexStride (sizeof(Point)), - mFetchTriangle (&MeshInterface::FetchTriangleFromSingles), - mFetchExTriangle (&MeshInterface::FetchExTriangleFromSingles), - #endif mTris (null), mVerts (null) -#endif { } @@ -164,11 +128,7 @@ MeshInterface::~MeshInterface() bool MeshInterface::IsValid() const { if(!mNbTris || !mNbVerts) return false; -#ifdef OPC_USE_CALLBACKS - if(!mObjCallback) return false; -#else if(!mTris || !mVerts) return false; -#endif return true; } @@ -188,13 +148,12 @@ udword MeshInterface::CheckTopology() const udword NbDegenerate = 0; VertexPointers VP; - ConversionArea VC; // Using callbacks, we don't have access to vertex indices. Nevertheless we still can check for // redundant vertex pointers, which cover all possibilities (callbacks/pointers/strides). for(udword i=0;imVRef[0] * VertexStride); - vp.Vertex[1] = (const Point*)(((ubyte*)Verts) + T->mVRef[1] * VertexStride); - vp.Vertex[2] = (const Point*)(((ubyte*)Verts) + T->mVRef[2] * VertexStride); -} - -void MeshInterface::FetchTriangleFromDoubles(VertexPointers& vp, udword index, ConversionArea vc) const -{ - const IndexedTriangle* T = (const IndexedTriangle*)(((ubyte*)mTris) + index * mTriStride); - - const Point* Verts = GetVerts(); - udword VertexStride = GetVertexStride(); - - for (int i = 0; i < 3; i++){ - const double* v = (const double*)(((ubyte*)Verts) + T->mVRef[i] * VertexStride); - - vc[i].x = (float)v[0]; - vc[i].y = (float)v[1]; - vc[i].z = (float)v[2]; - vp.Vertex[i] = &vc[i]; - } -} - -void MeshInterface::FetchExTriangleFromSingles(VertexPointersEx& vpe, udword index, ConversionArea vc) const -{ - const IndexedTriangle* T = (const IndexedTriangle*)(((ubyte*)mTris) + index * mTriStride); - - const Point* Verts = GetVerts(); - udword VertexStride = GetVertexStride(); - - dTriIndex VertIndex0 = T->mVRef[0]; - vpe.Index[0] = VertIndex0; - vpe.vp.Vertex[0] = (const Point*)(((ubyte*)Verts) + VertIndex0 * VertexStride); - - dTriIndex VertIndex1 = T->mVRef[1]; - vpe.Index[1] = VertIndex1; - vpe.vp.Vertex[1] = (const Point*)(((ubyte*)Verts) + VertIndex1 * VertexStride); - - dTriIndex VertIndex2 = T->mVRef[2]; - vpe.Index[2] = VertIndex2; - vpe.vp.Vertex[2] = (const Point*)(((ubyte*)Verts) + VertIndex2 * VertexStride); -} - -void MeshInterface::FetchExTriangleFromDoubles(VertexPointersEx& vpe, udword index, ConversionArea vc) const -{ - const IndexedTriangle* T = (const IndexedTriangle*)(((ubyte*)mTris) + index * mTriStride); - - const Point* Verts = GetVerts(); - udword VertexStride = GetVertexStride(); - - for (int i = 0; i < 3; i++){ - dTriIndex VertIndex = T->mVRef[i]; - vpe.Index[i] = VertIndex; - - const double* v = (const double*)(((ubyte*)Verts) + VertIndex * VertexStride); - vc[i].x = (float)v[0]; - vc[i].y = (float)v[1]; - vc[i].z = (float)v[2]; - vpe.vp.Vertex[i] = &vc[i]; - } -} -#endif -#endif - /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** @@ -362,18 +194,10 @@ bool MeshInterface::RemapClient(udword nb_indices, const dTriIndex* permutation) if(!nb_indices || !permutation) return false; if(nb_indices!=mNbTris) return false; -#ifdef OPC_USE_CALLBACKS - // We can't really do that using callbacks - return false; -#else IndexedTriangle* Tmp = new IndexedTriangle[mNbTris]; CHECKALLOC(Tmp); - #ifdef OPC_USE_STRIDE - udword Stride = mTriStride; - #else udword Stride = sizeof(IndexedTriangle); - #endif for(udword i=0;imVRef[0]]; - vp.Vertex[1] = &Verts[T->mVRef[1]]; - vp.Vertex[2] = &Verts[T->mVRef[2]]; - #endif -#endif + vp.Vertex[0] = &mVerts[T->mVRef[0]]; + vp.Vertex[1] = &mVerts[T->mVRef[1]]; + vp.Vertex[2] = &mVerts[T->mVRef[2]]; } - inline_ bool GetExTriangle(VertexPointersEx& vpe, udword index, ConversionArea vc) const + inline_ bool GetExTriangle(VertexPointersEx& vpe, udword index) const { -#ifdef OPC_USE_CALLBACKS - if (mObjExCallback) { (mObjExCallback)(index, vpe, mUserData); return true; } - else { (mObjCallback)(index, vpe.vp, mUserData); return false; } -#else - #ifdef OPC_USE_STRIDE - // Since there was conditional statement "if (Single)" which was unpredictable for compiler - // and required both branches to be always generated what made inlining a questionable - // benefit, I consider it better to introduce a forced call - // but get rig of branching and dead code injection. - ((*this).*mFetchExTriangle)(vpe, index, vc); - return true; - #else - const Point* Verts = GetVerts(); const IndexedTriangle* T = &mTris[index]; dTriIndex VertIndex0 = T->mVRef[0]; vpe.Index[0] = VertIndex0; - vpe.vp.Vertex[0] = &Verts[VertIndex0]; + vpe.vp.Vertex[0] = &mVerts[VertIndex0]; dTriIndex VertIndex1 = T->mVRef[1]; vpe.Index[1] = VertIndex1; - vpe.vp.Vertex[1] = &Verts[VertIndex1]; + vpe.vp.Vertex[1] = &mVerts[VertIndex1]; dTriIndex VertIndex2 = T->mVRef[2]; vpe.Index[2] = VertIndex2; - vpe.vp.Vertex[2] = &Verts[VertIndex2]; + vpe.vp.Vertex[2] = &mVerts[VertIndex2]; return true; - #endif -#endif } - private: -#ifndef OPC_USE_CALLBACKS - #ifdef OPC_USE_STRIDE - void FetchTriangleFromSingles(VertexPointers& vp, udword index, ConversionArea vc) const; - void FetchTriangleFromDoubles(VertexPointers& vp, udword index, ConversionArea vc) const; - void FetchExTriangleFromSingles(VertexPointersEx& vpe, udword index, ConversionArea vc) const; - void FetchExTriangleFromDoubles(VertexPointersEx& vpe, udword index, ConversionArea vc) const; - #endif -#endif - public: /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** @@ -256,25 +137,9 @@ udword mNbTris; //!< Number of triangles in the input model udword mNbVerts; //!< Number of vertices in the input model -#ifdef OPC_USE_CALLBACKS - // User callback - void* mUserData; //!< User-defined data sent to callback - RequestCallback mObjCallback; //!< Object callback - void* mExUserData; //!< User-defined data sent to ex-callback - RequestExCallback mObjExCallback; //!< Object ex-callback -#else // User pointers - #ifdef OPC_USE_STRIDE - udword mTriStride; //!< Possible triangle stride in bytes [Opcode 1.3] - udword mVertexStride; //!< Possible vertex stride in bytes [Opcode 1.3] - typedef void (MeshInterface:: *TriangleFetchProc)(VertexPointers& vp, udword index, ConversionArea vc) const; - TriangleFetchProc mFetchTriangle; - typedef void (MeshInterface:: *ExTriangleFetchProc)(VertexPointersEx& vpe, udword index, ConversionArea vc) const; - ExTriangleFetchProc mFetchExTriangle; - #endif const IndexedTriangle* mTris; //!< Array of indexed triangles const Point* mVerts; //!< Array of vertices -#endif }; #endif //__OPC_MESHINTERFACE_H__ diff --git a/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_Model.cpp b/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_Model.cpp index 418dd7e9..9f597dbd 100644 --- a/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_Model.cpp +++ b/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_Model.cpp @@ -18,10 +18,7 @@ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * The main collision wrapper, for all trees. Supported trees are: - * - Normal trees (2*N-1 nodes, full size) * - No-leaf trees (N-1 nodes, full size) - * - Quantized trees (2*N-1 nodes, half size) - * - Quantized no-leaf trees (N-1 nodes, half size) * * Usage: * @@ -37,9 +34,7 @@ * OPCODECREATE OPCC; * OPCC.IMesh = ...; * OPCC.Rules = ...; - * OPCC.NoLeaf = ...; - * OPCC.Quantized = ...; - * OPCC.KeepOriginal = ...; + * OPCC.KeepOriginal = ...; * bool Status = Sample.Build(OPCC); * \endcode * @@ -101,9 +96,6 @@ using namespace Opcode; /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// Model::Model() { -#ifdef __MESHMERIZER_H__ // Collision hulls only supported within ICE ! - mHull = null; -#endif // __MESHMERIZER_H__ } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -124,9 +116,6 @@ Model::~Model() void Model::Release() { ReleaseBase(); -#ifdef __MESHMERIZER_H__ // Collision hulls only supported within ICE ! - DELETESINGLE(mHull); -#endif // __MESHMERIZER_H__ } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -179,33 +168,13 @@ bool Model::Build(const OPCODECREATE& create) } // 3) Create an optimized tree according to user-settings - if(!CreateTree(create.mNoLeaf, create.mQuantized)) return false; + if(!CreateTree()) return false; // 3-2) Create optimized tree if(!mTree->Build(mSource)) return false; // 3-3) Delete generic tree if needed if(!create.mKeepOriginal) DELETESINGLE(mSource); - -#ifdef __MESHMERIZER_H__ - // 4) Convex hull - if(create.mCollisionHull) - { - // Create hull - mHull = new CollisionHull; - CHECKALLOC(mHull); - - CONVEXHULLCREATE CHC; - // ### doesn't work with strides - CHC.NbVerts = create.mIMesh->GetNbVertices(); - CHC.Vertices = create.mIMesh->GetVerts(); - CHC.UnifyNormals = true; - CHC.ReduceVertices = true; - CHC.WordFaces = false; - mHull->Compute(CHC); - } -#endif // __MESHMERIZER_H__ - return true; } diff --git a/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_Model.h b/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_Model.h index 98dee560..af375b96 100644 --- a/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_Model.h +++ b/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_Model.h @@ -36,16 +36,6 @@ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// override(BaseModel) bool Build(const OPCODECREATE& create); -#ifdef __MESHMERIZER_H__ - /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - /** - * Gets the collision hull. - * \return the collision hull if it exists - */ - /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - inline_ const CollisionHull* GetHull() const { return mHull; } -#endif // __MESHMERIZER_H__ - /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Gets the number of bytes used by the tree. @@ -55,9 +45,6 @@ override(BaseModel) udword GetUsedBytes() const; private: -#ifdef __MESHMERIZER_H__ - CollisionHull* mHull; //!< Possible convex hull -#endif // __MESHMERIZER_H__ // Internal methods void Release(); }; diff --git a/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_OBBCollider.cpp b/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_OBBCollider.cpp index 4c554f9b..076ed5db 100644 --- a/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_OBBCollider.cpp +++ b/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_OBBCollider.cpp @@ -43,7 +43,7 @@ using namespace Opcode; //! OBB-triangle test #define OBB_PRIM(prim_index, flag) \ /* Request vertices from the app */ \ - VertexPointers VP; ConversionArea VC; mIMesh->GetTriangle(VP, prim_index, VC); \ + VertexPointers VP; mIMesh->GetTriangle(VP, prim_index); \ /* Transform them in a common space */ \ TransformPoint(mLeafVerts[0], *VP.Vertex[0], mRModelToBox, mTModelToBox); \ TransformPoint(mLeafVerts[1], *VP.Vertex[1], mRModelToBox, mTModelToBox); \ @@ -109,52 +109,11 @@ bool OBBCollider::Collide(OBBCache& cache, const OBB& box, const Model& model, c // Init collision query if(InitQuery(cache, box, worldb, worldm)) return true; - if(!model.HasLeafNodes()) - { - if(model.IsQuantized()) - { - const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree(); - - // Setup dequantization coeffs - mCenterCoeff = Tree->mCenterCoeff; - mExtentsCoeff = Tree->mExtentsCoeff; - - // Perform collision query - if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes()); - else _Collide(Tree->GetNodes()); - } - else - { const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree(); // Perform collision query if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes()); else _Collide(Tree->GetNodes()); - } - } - else - { - if(model.IsQuantized()) - { - const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree(); - - // Setup dequantization coeffs - mCenterCoeff = Tree->mCenterCoeff; - mExtentsCoeff = Tree->mExtentsCoeff; - - // Perform collision query - if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes()); - else _Collide(Tree->GetNodes()); - } - else - { - const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree(); - - // Perform collision query - if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes()); - else _Collide(Tree->GetNodes()); - } - } return true; } @@ -671,48 +630,10 @@ bool HybridOBBCollider::Collide(OBBCache& cache, const OBB& box, const HybridMod mTouchedPrimitives = &mTouchedBoxes; // Now, do the actual query against leaf boxes - if(!model.HasLeafNodes()) - { - if(model.IsQuantized()) - { - const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree(); - - // Setup dequantization coeffs - mCenterCoeff = Tree->mCenterCoeff; - mExtentsCoeff = Tree->mExtentsCoeff; - - // Perform collision query - we don't want primitive tests here! - _CollideNoPrimitiveTest(Tree->GetNodes()); - } - else - { const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree(); // Perform collision query - we don't want primitive tests here! _CollideNoPrimitiveTest(Tree->GetNodes()); - } - } - else - { - if(model.IsQuantized()) - { - const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree(); - - // Setup dequantization coeffs - mCenterCoeff = Tree->mCenterCoeff; - mExtentsCoeff = Tree->mExtentsCoeff; - - // Perform collision query - we don't want primitive tests here! - _CollideNoPrimitiveTest(Tree->GetNodes()); - } - else - { - const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree(); - - // Perform collision query - we don't want primitive tests here! - _CollideNoPrimitiveTest(Tree->GetNodes()); - } - } // We only have a list of boxes so far if(GetContactStatus()) diff --git a/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_OptimizedTree.cpp b/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_OptimizedTree.cpp index 460d440a..005973d1 100644 --- a/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_OptimizedTree.cpp +++ b/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_OptimizedTree.cpp @@ -8,11 +8,9 @@ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** - * Contains code for optimized trees. Implements 4 trees: + * Contains code for optimized trees. Implements 2 trees: * - normal * - no leaf - * - quantized - * - no leaf / quantized * * \file OPC_OptimizedTree.cpp * \author Pierre Terdiman @@ -42,27 +40,6 @@ */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/** - * A quantized AABB tree. - * - * \class AABBQuantizedTree - * \author Pierre Terdiman - * \version 1.3 - * \date March, 20, 2001 -*/ -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/** - * A quantized no-leaf AABB tree. - * - * \class AABBQuantizedNoLeafTree - * \author Pierre Terdiman - * \version 1.3 - * \date March, 20, 2001 -*/ -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Precompiled Header @@ -70,11 +47,6 @@ using namespace Opcode; -//! Compilation flag: -//! - true to fix quantized boxes (i.e. make sure they enclose the original ones) -//! - false to see the effects of quantization errors (faster, but wrong results in some cases) -static const bool gFixQuantized = true; - /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Builds an implicit tree from a standard one. An implicit tree is a complete tree (2*N-1 nodes) whose negative @@ -96,6 +68,7 @@ static const bool gFixQuantized = true; * \param current_node [in] current node from input tree */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/* static void _BuildCollisionTree(AABBCollisionNode* linear, const udword box_id, udword& current_id, const AABBTreeNode* current_node) { // Current node from input tree is "current_node". Must be flattened into "linear[boxid]". @@ -127,6 +100,7 @@ static void _BuildCollisionTree(AABBCollisionNode* linear, const udword box_id, _BuildCollisionTree(linear, NegID, current_id, current_node->GetNeg()); } } +*/ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** @@ -206,20 +180,22 @@ static void _BuildNoLeafTree(AABBNoLeafNode* linear, const udword box_id, udword * Constructor. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/* AABBCollisionTree::AABBCollisionTree() : mNodes(null) { } - +*/ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Destructor. */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/* AABBCollisionTree::~AABBCollisionTree() { DELETEARRAY(mNodes); } - +*/ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Builds the collision tree from a generic AABB tree. @@ -227,6 +203,7 @@ AABBCollisionTree::~AABBCollisionTree() * \return true if success */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/* bool AABBCollisionTree::Build(AABBTree* tree) { // Checkings @@ -252,7 +229,7 @@ bool AABBCollisionTree::Build(AABBTree* tree) return true; } - +*/ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Refits the collision tree after vertices have been modified. @@ -260,12 +237,13 @@ bool AABBCollisionTree::Build(AABBTree* tree) * \return true if success */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -bool AABBCollisionTree::Refit(const MeshInterface* /*mesh_interface*/) +/* +bool AABBCollisionTree::Refit(const MeshInterface* ) { ASSERT(!"Not implemented since AABBCollisionTrees have twice as more nodes to refit as AABBNoLeafTrees!"); return false; } - +*/ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Walks the tree and call the user back for each node. @@ -274,6 +252,7 @@ bool AABBCollisionTree::Refit(const MeshInterface* /*mesh_interface*/) * \return true if success */ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/* bool AABBCollisionTree::Walk(GenericWalkingCallback callback, void* user_data) const { if(!callback) return false; @@ -294,7 +273,7 @@ bool AABBCollisionTree::Walk(GenericWalkingCallback callback, void* user_data) c Local::_Walk(mNodes, callback, user_data); return true; } - +*/ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** @@ -348,26 +327,11 @@ bool AABBNoLeafTree::Build(AABBTree* tree) return true; } -inline_ void ComputeMinMax(Point& min, Point& max, const VertexPointers& vp) +inline_ void lComputeMinMax(Point& min, Point& max, const VertexPointers& vp) { - // Compute triangle's AABB = a leaf box -#ifdef OPC_USE_FCOMI // a 15% speedup on my machine, not much - min.x = FCMin3(vp.Vertex[0]->x, vp.Vertex[1]->x, vp.Vertex[2]->x); - max.x = FCMax3(vp.Vertex[0]->x, vp.Vertex[1]->x, vp.Vertex[2]->x); - - min.y = FCMin3(vp.Vertex[0]->y, vp.Vertex[1]->y, vp.Vertex[2]->y); - max.y = FCMax3(vp.Vertex[0]->y, vp.Vertex[1]->y, vp.Vertex[2]->y); - - min.z = FCMin3(vp.Vertex[0]->z, vp.Vertex[1]->z, vp.Vertex[2]->z); - max.z = FCMax3(vp.Vertex[0]->z, vp.Vertex[1]->z, vp.Vertex[2]->z); -#else - min = *vp.Vertex[0]; - max = *vp.Vertex[0]; - min.Min(*vp.Vertex[1]); - max.Max(*vp.Vertex[1]); - min.Min(*vp.Vertex[2]); - max.Max(*vp.Vertex[2]); -#endif + MinMax(min.x, max.x, vp.Vertex[0]->x, vp.Vertex[1]->x, vp.Vertex[2]->x); + MinMax(min.y, max.y, vp.Vertex[0]->y, vp.Vertex[1]->y, vp.Vertex[2]->y); + MinMax(min.z, max.z, vp.Vertex[0]->z, vp.Vertex[1]->z, vp.Vertex[2]->z); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -384,7 +348,6 @@ bool AABBNoLeafTree::Refit(const MeshInterface* mesh_interface) // Bottom-up update VertexPointers VP; - ConversionArea VC; Point Min,Max; Point Min_,Max_; udword Index = mNbNodes; @@ -394,8 +357,8 @@ bool AABBNoLeafTree::Refit(const MeshInterface* mesh_interface) if(Current.HasPosLeaf()) { - mesh_interface->GetTriangle(VP, Current.GetPosPrimitive(), VC); - ComputeMinMax(Min, Max, VP); + mesh_interface->GetTriangle(VP, Current.GetPosPrimitive()); + lComputeMinMax(Min, Max, VP); } else { @@ -406,8 +369,8 @@ bool AABBNoLeafTree::Refit(const MeshInterface* mesh_interface) if(Current.HasNegLeaf()) { - mesh_interface->GetTriangle(VP, Current.GetNegPrimitive(), VC); - ComputeMinMax(Min_, Max_, VP); + mesh_interface->GetTriangle(VP, Current.GetNegPrimitive()); + lComputeMinMax(Min_, Max_, VP); } else { @@ -415,22 +378,27 @@ bool AABBNoLeafTree::Refit(const MeshInterface* mesh_interface) CurrentBox.GetMin(Min_); CurrentBox.GetMax(Max_); } -#ifdef OPC_USE_FCOMI - Min.x = FCMin2(Min.x, Min_.x); - Max.x = FCMax2(Max.x, Max_.x); - Min.y = FCMin2(Min.y, Min_.y); - Max.y = FCMax2(Max.y, Max_.y); - Min.z = FCMin2(Min.z, Min_.z); - Max.z = FCMax2(Max.z, Max_.z); -#else - Min.Min(Min_); - Max.Max(Max_); -#endif - Current.mAABB.SetMinMax(Min, Max); + + if (Min_.x < Min.x) + Min.x = Min_.x; + if (Min_.y < Min.y) + Min.y = Min_.y; + if (Min_.z < Min.z) + Min.z = Min_.z; + + if (Max_.x > Max.x) + Max.x = Max_.x; + if (Max_.y > Max.y) + Max.y = Max_.y; + if (Max_.z > Max.z) + Max.z = Max_.z; + + Current.mAABB.SetMinMax(Min, Max); } return true; } + /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * Walks the tree and call the user back for each node. @@ -457,327 +425,3 @@ bool AABBNoLeafTree::Walk(GenericWalkingCallback callback, void* user_data) cons return true; } -// Quantization notes: -// - We could use the highest bits of mData to store some more quantized bits. Dequantization code -// would be slightly more complex, but number of overlap tests would be reduced (and anyhow those -// bits are currently wasted). Of course it's not possible if we move to 16 bits mData. -// - Something like "16 bits floats" could be tested, to bypass the int-to-float conversion. -// - A dedicated BV-BV test could be used, dequantizing while testing for overlap. (i.e. it's some -// lazy-dequantization which may save some work in case of early exits). At the very least some -// muls could be saved by precomputing several more matrices. But maybe not worth the pain. -// - Do we need to dequantize anyway? Not doing the extents-related muls only implies the box has -// been scaled, for example. -// - The deeper we move into the hierarchy, the smaller the extents should be. May not need a fixed -// number of quantization bits. Even better, could probably be best delta-encoded. - - -// Find max values. Some people asked why I wasn't simply using the first node. Well, I can't. -// I'm not looking for (min, max) values like in a standard AABB, I'm looking for the extremal -// centers/extents in order to quantize them. The first node would only give a single center and -// a single extents. While extents would be the biggest, the center wouldn't. -#define FIND_MAX_VALUES \ - /* Get max values */ \ - Point CMax(MIN_FLOAT, MIN_FLOAT, MIN_FLOAT); \ - Point EMax(MIN_FLOAT, MIN_FLOAT, MIN_FLOAT); \ - for(udword i=0;iCMax.x) CMax.x = fabsf(Nodes[i].mAABB.mCenter.x); \ - if(fabsf(Nodes[i].mAABB.mCenter.y)>CMax.y) CMax.y = fabsf(Nodes[i].mAABB.mCenter.y); \ - if(fabsf(Nodes[i].mAABB.mCenter.z)>CMax.z) CMax.z = fabsf(Nodes[i].mAABB.mCenter.z); \ - if(fabsf(Nodes[i].mAABB.mExtents.x)>EMax.x) EMax.x = fabsf(Nodes[i].mAABB.mExtents.x); \ - if(fabsf(Nodes[i].mAABB.mExtents.y)>EMax.y) EMax.y = fabsf(Nodes[i].mAABB.mExtents.y); \ - if(fabsf(Nodes[i].mAABB.mExtents.z)>EMax.z) EMax.z = fabsf(Nodes[i].mAABB.mExtents.z); \ - } - -#define INIT_QUANTIZATION \ - udword nbc=15; /* Keep one bit for sign */ \ - udword nbe=15; /* Keep one bit for fix */ \ - if(!gFixQuantized) nbe++; \ - \ - /* Compute quantization coeffs */ \ - Point CQuantCoeff, EQuantCoeff; \ - CQuantCoeff.x = CMax.x!=0.0f ? float((1<Min[j]) mNodes[i].mAABB.mExtents[j]++; \ - else FixMe=false; \ - /* Prevent wrapping */ \ - if(!mNodes[i].mAABB.mExtents[j]) \ - { \ - mNodes[i].mAABB.mExtents[j]=0xffff; \ - FixMe=false; \ - } \ - }while(FixMe); \ - } \ - } - -#define REMAP_DATA(member) \ - /* Fix data */ \ - Data = Nodes[i].member; \ - if(!(Data&1)) \ - { \ - /* Compute box number */ \ - size_t Nb = (Data - size_t(Nodes))/Nodes[i].GetNodeSize(); \ - Data = (size_t) &mNodes[Nb]; \ - } \ - /* ...remapped */ \ - mNodes[i].member = Data; - -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/** - * Constructor. - */ -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -AABBQuantizedTree::AABBQuantizedTree() : mNodes(null) -{ -} - -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/** - * Destructor. - */ -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -AABBQuantizedTree::~AABBQuantizedTree() -{ - DELETEARRAY(mNodes); -} - -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/** - * Builds the collision tree from a generic AABB tree. - * \param tree [in] generic AABB tree - * \return true if success - */ -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -bool AABBQuantizedTree::Build(AABBTree* tree) -{ - // Checkings - if(!tree) return false; - // Check the input tree is complete - udword NbTriangles = tree->GetNbPrimitives(); - udword NbNodes = tree->GetNbNodes(); - if(NbNodes!=NbTriangles*2-1) return false; - - // Get nodes - mNbNodes = NbNodes; - DELETEARRAY(mNodes); - AABBCollisionNode* Nodes = new AABBCollisionNode[mNbNodes]; - CHECKALLOC(Nodes); - - // Build the tree - udword CurID = 1; - _BuildCollisionTree(Nodes, 0, CurID, tree); - - // Quantize - { - mNodes = new AABBQuantizedNode[mNbNodes]; - CHECKALLOC(mNodes); - - // Get max values - FIND_MAX_VALUES - - // Quantization - INIT_QUANTIZATION - - // Quantize - size_t Data; - for(udword i=0;iIsLeaf()) - { - _Walk(current_node->GetPos(), callback, user_data); - _Walk(current_node->GetNeg(), callback, user_data); - } - } - }; - Local::_Walk(mNodes, callback, user_data); - return true; -} - - - -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/** - * Constructor. - */ -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -AABBQuantizedNoLeafTree::AABBQuantizedNoLeafTree() : mNodes(null) -{ -} - -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/** - * Destructor. - */ -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -AABBQuantizedNoLeafTree::~AABBQuantizedNoLeafTree() -{ - DELETEARRAY(mNodes); -} - -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/** - * Builds the collision tree from a generic AABB tree. - * \param tree [in] generic AABB tree - * \return true if success - */ -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -bool AABBQuantizedNoLeafTree::Build(AABBTree* tree) -{ - // Checkings - if(!tree) return false; - // Check the input tree is complete - udword NbTriangles = tree->GetNbPrimitives(); - udword NbNodes = tree->GetNbNodes(); - if(NbNodes!=NbTriangles*2-1) return false; - - // Get nodes - mNbNodes = NbTriangles-1; - DELETEARRAY(mNodes); - AABBNoLeafNode* Nodes = new AABBNoLeafNode[mNbNodes]; - CHECKALLOC(Nodes); - - // Build the tree - udword CurID = 1; - _BuildNoLeafTree(Nodes, 0, CurID, tree); - ASSERT(CurID==mNbNodes); - - // Quantize - { - mNodes = new AABBQuantizedNoLeafNode[mNbNodes]; - CHECKALLOC(mNodes); - - // Get max values - FIND_MAX_VALUES - - // Quantization - INIT_QUANTIZATION - - // Quantize - size_t Data; - for(udword i=0;iHasPosLeaf()) _Walk(current_node->GetPos(), callback, user_data); - if(!current_node->HasNegLeaf()) _Walk(current_node->GetNeg(), callback, user_data); - } - }; - Local::_Walk(mNodes, callback, user_data); - return true; -} diff --git a/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_OptimizedTree.h b/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_OptimizedTree.h index 36aea071..35e9c2f3 100644 --- a/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_OptimizedTree.h +++ b/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_OptimizedTree.h @@ -185,22 +185,4 @@ IMPLEMENT_COLLISION_TREE(AABBNoLeafTree, AABBNoLeafNode) }; - class OPCODE_API AABBQuantizedTree : public AABBOptimizedTree - { - IMPLEMENT_COLLISION_TREE(AABBQuantizedTree, AABBQuantizedNode) - - public: - Point mCenterCoeff; - Point mExtentsCoeff; - }; - - class OPCODE_API AABBQuantizedNoLeafTree : public AABBOptimizedTree - { - IMPLEMENT_COLLISION_TREE(AABBQuantizedNoLeafTree, AABBQuantizedNoLeafNode) - - public: - Point mCenterCoeff; - Point mExtentsCoeff; - }; - #endif // __OPC_OPTIMIZEDTREE_H__ diff --git a/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_PlanesCollider.cpp b/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_PlanesCollider.cpp index 25d1b0b1..1f97f5bd 100644 --- a/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_PlanesCollider.cpp +++ b/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_PlanesCollider.cpp @@ -43,7 +43,7 @@ using namespace Opcode; //! Planes-triangle test #define PLANES_PRIM(prim_index, flag) \ /* Request vertices from the app */ \ - mIMesh->GetTriangle(mVP, prim_index, mVC); \ + mIMesh->GetTriangle(mVP, prim_index); \ /* Perform triangle-box overlap test */ \ if(PlanesTriOverlap(clip_mask)) \ { \ @@ -110,52 +110,11 @@ bool PlanesCollider::Collide(PlanesCache& cache, const Plane* planes, udword nb_ udword PlaneMask = (1<mCenterCoeff; - mExtentsCoeff = Tree->mExtentsCoeff; - - // Perform collision query - if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes(), PlaneMask); - else _Collide(Tree->GetNodes(), PlaneMask); - } - else - { const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree(); // Perform collision query if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes(), PlaneMask); else _Collide(Tree->GetNodes(), PlaneMask); - } - } - else - { - if(model.IsQuantized()) - { - const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree(); - - // Setup dequantization coeffs - mCenterCoeff = Tree->mCenterCoeff; - mExtentsCoeff = Tree->mExtentsCoeff; - - // Perform collision query - if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes(), PlaneMask); - else _Collide(Tree->GetNodes(), PlaneMask); - } - else - { - const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree(); - - // Perform collision query - if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes(), PlaneMask); - else _Collide(Tree->GetNodes(), PlaneMask); - } - } return true; } @@ -556,48 +515,10 @@ bool HybridPlanesCollider::Collide(PlanesCache& cache, const Plane* planes, udwo udword PlaneMask = (1<mCenterCoeff; - mExtentsCoeff = Tree->mExtentsCoeff; - - // Perform collision query - we don't want primitive tests here! - _CollideNoPrimitiveTest(Tree->GetNodes(), PlaneMask); - } - else - { const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree(); // Perform collision query - we don't want primitive tests here! _CollideNoPrimitiveTest(Tree->GetNodes(), PlaneMask); - } - } - else - { - if(model.IsQuantized()) - { - const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree(); - - // Setup dequantization coeffs - mCenterCoeff = Tree->mCenterCoeff; - mExtentsCoeff = Tree->mExtentsCoeff; - - // Perform collision query - we don't want primitive tests here! - _CollideNoPrimitiveTest(Tree->GetNodes(), PlaneMask); - } - else - { - const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree(); - - // Perform collision query - we don't want primitive tests here! - _CollideNoPrimitiveTest(Tree->GetNodes(), PlaneMask); - } - } // We only have a list of boxes so far if(GetContactStatus()) diff --git a/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_PlanesCollider.h b/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_PlanesCollider.h index 872f8c55..35de28a1 100644 --- a/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_PlanesCollider.h +++ b/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_PlanesCollider.h @@ -90,7 +90,6 @@ Plane* mPlanes; // Leaf description VertexPointers mVP; - ConversionArea mVC; // Internal methods void _Collide(const AABBCollisionNode* node, udword clip_mask); void _Collide(const AABBNoLeafNode* node, udword clip_mask); diff --git a/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_RayAABBOverlap.h b/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_RayAABBOverlap.h index a8162bf0..f318886a 100644 --- a/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_RayAABBOverlap.h +++ b/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_RayAABBOverlap.h @@ -42,22 +42,26 @@ inline_ BOOL RayCollider::RayAABBOverlap(const Point& center, const Point& exten // Stats mNbRayBVTests++; -// float Dx = mOrigin.x - center.x; if(fabsf(Dx) > extents.x && Dx*mDir.x>=0.0f) return FALSE; -// float Dy = mOrigin.y - center.y; if(fabsf(Dy) > extents.y && Dy*mDir.y>=0.0f) return FALSE; -// float Dz = mOrigin.z - center.z; if(fabsf(Dz) > extents.z && Dz*mDir.z>=0.0f) return FALSE; - - float Dx = mOrigin.x - center.x; if(GREATER(Dx, extents.x) && Dx*mDir.x>=0.0f) return FALSE; - float Dy = mOrigin.y - center.y; if(GREATER(Dy, extents.y) && Dy*mDir.y>=0.0f) return FALSE; - float Dz = mOrigin.z - center.z; if(GREATER(Dz, extents.z) && Dz*mDir.z>=0.0f) return FALSE; - -// float Dx = mOrigin.x - center.x; if(GREATER(Dx, extents.x) && ((SIR(Dx)-1)^SIR(mDir.x))>=0.0f) return FALSE; -// float Dy = mOrigin.y - center.y; if(GREATER(Dy, extents.y) && ((SIR(Dy)-1)^SIR(mDir.y))>=0.0f) return FALSE; -// float Dz = mOrigin.z - center.z; if(GREATER(Dz, extents.z) && ((SIR(Dz)-1)^SIR(mDir.z))>=0.0f) return FALSE; + float Dx = mOrigin.x - center.x; + if(fabsf(Dx) > extents.x && Dx*mDir.x>=0.0f) + return FALSE; + float Dy = mOrigin.y - center.y; + if(fabsf(Dy) > extents.y && Dy*mDir.y>=0.0f) + return FALSE; + float Dz = mOrigin.z - center.z; + if(fabsf(Dz) > extents.z && Dz*mDir.z>=0.0f) + return FALSE; float f; - f = mDir.y * Dz - mDir.z * Dy; if(fabsf(f) > extents.y*mFDir.z + extents.z*mFDir.y) return FALSE; - f = mDir.z * Dx - mDir.x * Dz; if(fabsf(f) > extents.x*mFDir.z + extents.z*mFDir.x) return FALSE; - f = mDir.x * Dy - mDir.y * Dx; if(fabsf(f) > extents.x*mFDir.y + extents.y*mFDir.x) return FALSE; + f = mDir.y * Dz - mDir.z * Dy; + if(fabsf(f) > extents.y*mFDir.z + extents.z*mFDir.y) + return FALSE; + f = mDir.z * Dx - mDir.x * Dz; + if(fabsf(f) > extents.x*mFDir.z + extents.z*mFDir.x) + return FALSE; + f = mDir.x * Dy - mDir.y * Dx; + if(fabsf(f) > extents.x*mFDir.y + extents.y*mFDir.x) + return FALSE; return TRUE; } diff --git a/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_RayCollider.cpp b/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_RayCollider.cpp index ad4b3284..46f80f4e 100644 --- a/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_RayCollider.cpp +++ b/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_RayCollider.cpp @@ -173,7 +173,7 @@ using namespace Opcode; #define SEGMENT_PRIM(prim_index, flag) \ /* Request vertices from the app */ \ - VertexPointers VP; ConversionArea VC; mIMesh->GetTriangle(VP, prim_index, VC); \ + VertexPointers VP; mIMesh->GetTriangle(VP, prim_index); \ \ /* Perform ray-tri overlap test and return */ \ if(RayTriOverlap(*VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2])) \ @@ -188,7 +188,7 @@ using namespace Opcode; #define RAY_PRIM(prim_index, flag) \ /* Request vertices from the app */ \ - VertexPointers VP; ConversionArea VC; mIMesh->GetTriangle(VP, prim_index, VC); \ + VertexPointers VP; mIMesh->GetTriangle(VP, prim_index); \ \ /* Perform ray-tri overlap test and return */ \ if(RayTriOverlap(*VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2])) \ @@ -268,52 +268,11 @@ bool RayCollider::Collide(const Ray& world_ray, const Model& model, const Matrix // Init collision query if(InitQuery(world_ray, world, cache)) return true; - if(!model.HasLeafNodes()) - { - if(model.IsQuantized()) - { - const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree(); - - // Setup dequantization coeffs - mCenterCoeff = Tree->mCenterCoeff; - mExtentsCoeff = Tree->mExtentsCoeff; - - // Perform stabbing query - if(IR(mMaxDist)!=IEEE_MAX_FLOAT) _SegmentStab(Tree->GetNodes()); - else _RayStab(Tree->GetNodes()); - } - else - { const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree(); // Perform stabbing query if(IR(mMaxDist)!=IEEE_MAX_FLOAT) _SegmentStab(Tree->GetNodes()); else _RayStab(Tree->GetNodes()); - } - } - else - { - if(model.IsQuantized()) - { - const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree(); - - // Setup dequantization coeffs - mCenterCoeff = Tree->mCenterCoeff; - mExtentsCoeff = Tree->mExtentsCoeff; - - // Perform stabbing query - if(IR(mMaxDist)!=IEEE_MAX_FLOAT) _SegmentStab(Tree->GetNodes()); - else _RayStab(Tree->GetNodes()); - } - else - { - const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree(); - - // Perform stabbing query - if(IR(mMaxDist)!=IEEE_MAX_FLOAT) _SegmentStab(Tree->GetNodes()); - else _RayStab(Tree->GetNodes()); - } - } // Update cache if needed UPDATE_CACHE diff --git a/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_Settings.h b/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_Settings.h index 75098887..3600378f 100644 --- a/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_Settings.h +++ b/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_Settings.h @@ -23,21 +23,12 @@ //! Use CPU comparisons (comment that line to use standard FPU compares) //#define OPC_CPU_COMPARE - //! Use FCOMI / FCMOV on Pentium-Pro based processors (comment that line to use plain C++) - #define OPC_USE_FCOMI - //! Use epsilon value in tri-tri overlap test #define OPC_TRITRI_EPSILON_TEST //! Use tree-coherence or not [not implemented yet] // #define OPC_USE_TREE_COHERENCE - //! Use callbacks or direct pointers. Using callbacks might be a bit slower (but probably not much) -// #define OPC_USE_CALLBACKS - - //! Support triangle and vertex strides or not. Using strides might be a bit slower (but probably not much) - #define OPC_USE_STRIDE - //! Discard negative pointer in vanilla trees #define OPC_NO_NEG_VANILLA_TREE diff --git a/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_SphereCollider.cpp b/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_SphereCollider.cpp index f18fe0ca..f3a48868 100644 --- a/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_SphereCollider.cpp +++ b/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_SphereCollider.cpp @@ -47,7 +47,7 @@ using namespace Opcode; //! Sphere-triangle overlap test #define SPHERE_PRIM(prim_index, flag) \ /* Request vertices from the app */ \ - VertexPointers VP; ConversionArea VC; mIMesh->GetTriangle(VP, prim_index, VC); \ + VertexPointers VP; mIMesh->GetTriangle(VP, prim_index); \ \ /* Perform sphere-tri overlap test */ \ if(SphereTriOverlap(*VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2])) \ @@ -112,52 +112,11 @@ bool SphereCollider::Collide(SphereCache& cache, const Sphere& sphere, const Mod return true; } - if(!model.HasLeafNodes()) - { - if(model.IsQuantized()) - { - const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree(); - - // Setup dequantization coeffs - mCenterCoeff = Tree->mCenterCoeff; - mExtentsCoeff = Tree->mExtentsCoeff; - - // Perform collision query - if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes()); - else _Collide(Tree->GetNodes()); - } - else - { const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree(); // Perform collision query if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes()); else _Collide(Tree->GetNodes()); - } - } - else - { - if(model.IsQuantized()) - { - const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree(); - - // Setup dequantization coeffs - mCenterCoeff = Tree->mCenterCoeff; - mExtentsCoeff = Tree->mExtentsCoeff; - - // Perform collision query - if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes()); - else _Collide(Tree->GetNodes()); - } - else - { - const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree(); - - // Perform collision query - if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes()); - else _Collide(Tree->GetNodes()); - } - } return true; } @@ -643,48 +602,10 @@ bool HybridSphereCollider::Collide(SphereCache& cache, const Sphere& sphere, con mTouchedPrimitives = &mTouchedBoxes; // Now, do the actual query against leaf boxes - if(!model.HasLeafNodes()) - { - if(model.IsQuantized()) - { - const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree(); - - // Setup dequantization coeffs - mCenterCoeff = Tree->mCenterCoeff; - mExtentsCoeff = Tree->mExtentsCoeff; - - // Perform collision query - we don't want primitive tests here! - _CollideNoPrimitiveTest(Tree->GetNodes()); - } - else - { const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree(); // Perform collision query - we don't want primitive tests here! _CollideNoPrimitiveTest(Tree->GetNodes()); - } - } - else - { - if(model.IsQuantized()) - { - const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree(); - - // Setup dequantization coeffs - mCenterCoeff = Tree->mCenterCoeff; - mExtentsCoeff = Tree->mExtentsCoeff; - - // Perform collision query - we don't want primitive tests here! - _CollideNoPrimitiveTest(Tree->GetNodes()); - } - else - { - const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree(); - - // Perform collision query - we don't want primitive tests here! - _CollideNoPrimitiveTest(Tree->GetNodes()); - } - } // We only have a list of boxes so far if(GetContactStatus()) diff --git a/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_TreeBuilders.cpp b/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_TreeBuilders.cpp index 4a78914b..a25aec0d 100644 --- a/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_TreeBuilders.cpp +++ b/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_TreeBuilders.cpp @@ -131,11 +131,10 @@ bool AABBTreeOfTrianglesBuilder::ComputeGlobalBox(const dTriIndex* primitives, u // Loop through triangles VertexPointers VP; - ConversionArea VC; while(nb_prims--) { // Get current triangle-vertices - mIMesh->GetTriangle(VP, *primitives++, VC); + mIMesh->GetTriangle(VP, *primitives++); // Update global box Min.Min(*VP.Vertex[0]).Min(*VP.Vertex[1]).Min(*VP.Vertex[2]); Max.Max(*VP.Vertex[0]).Max(*VP.Vertex[1]).Max(*VP.Vertex[2]); @@ -154,8 +153,7 @@ bool AABBTreeOfTrianglesBuilder::ComputeGlobalBox(const dTriIndex* primitives, u Point AABBTreeOfTrianglesBuilder::GetSplittingValues(udword index) const { VertexPointers VP; - ConversionArea VC; - mIMesh->GetTriangle(VP, index, VC); + mIMesh->GetTriangle(VP, index); return ( *VP.Vertex[0] + *VP.Vertex[1] + *VP.Vertex[2] ) * INV3; } @@ -183,8 +181,7 @@ float AABBTreeOfTrianglesBuilder::GetSplittingValue(udword index, udword axis) c // +mVerts[mTriList[index].mVRef[2]][axis])*INV3; VertexPointers VP; - ConversionArea VC; - mIMesh->GetTriangle(VP, index, VC); + mIMesh->GetTriangle(VP, index); // Compute correct component from center of triangle return ((*VP.Vertex[0])[axis] @@ -209,11 +206,10 @@ float AABBTreeOfTrianglesBuilder::GetSplittingValue(const dTriIndex* primitives, // Loop through triangles float SplitValue = 0.0f; VertexPointers VP; - ConversionArea VC; for(udword i=0;iGetTriangle(VP, primitives[i], VC); + mIMesh->GetTriangle(VP, primitives[i]); // Update split value SplitValue += (*VP.Vertex[0])[axis]; SplitValue += (*VP.Vertex[1])[axis]; diff --git a/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_TreeCollider.cpp b/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_TreeCollider.cpp index 5668ef57..2e66ce2b 100644 --- a/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_TreeCollider.cpp +++ b/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_TreeCollider.cpp @@ -92,132 +92,23 @@ bool AABBTreeCollider::Collide(BVTCache& cache, const Matrix4x4* world0, const M { // Checkings if(!cache.Model0 || !cache.Model1) return false; - if(cache.Model0->HasLeafNodes()!=cache.Model1->HasLeafNodes()) return false; - if(cache.Model0->IsQuantized()!=cache.Model1->IsQuantized()) return false; /* Rules: - - perform hull test - - when hulls collide, disable hull test - if meshes overlap, reset countdown - if countdown reaches 0, enable hull test */ -#ifdef __MESHMERIZER_H__ - // Handle hulls - if(cache.HullTest) - { - if(cache.Model0->GetHull() && cache.Model1->GetHull()) - { - struct Local - { - static Point* SVCallback(const Point& sv, udword& previndex, udword user_data) - { - CollisionHull* Hull = (CollisionHull*)user_data; - previndex = Hull->ComputeSupportingVertex(sv, previndex); - return (Point*)&Hull->GetVerts()[previndex]; - } - }; - - bool Collide; - - if(0) - { - static GJKEngine GJK; -- not thread safe, store in ThreadLocalData - static bool GJKInitDone=false; -- not thread safe, to be removed - if(!GJKInitDone) - { - GJK.Enable(GJK_BACKUP_PROCEDURE); - GJK.Enable(GJK_DEGENERATE); - GJK.Enable(GJK_HILLCLIMBING); - GJKInitDone = true; - } - GJK.SetCallbackObj0(Local::SVCallback); - GJK.SetCallbackObj1(Local::SVCallback); - GJK.SetUserData0(udword(cache.Model0->GetHull())); - GJK.SetUserData1(udword(cache.Model1->GetHull())); - Collide = GJK.Collide(*world0, *world1, &cache.SepVector); - } - else - { - static SVEngine SVE; -- not thread safe, store in ThreadLocalData - SVE.SetCallbackObj0(Local::SVCallback); - SVE.SetCallbackObj1(Local::SVCallback); - SVE.SetUserData0(udword(cache.Model0->GetHull())); - SVE.SetUserData1(udword(cache.Model1->GetHull())); - Collide = SVE.Collide(*world0, *world1, &cache.SepVector); - } - - if(!Collide) - { - // Reset stats & contact status - mFlags &= ~OPC_CONTACT; - mNbBVBVTests = 0; - mNbPrimPrimTests = 0; - mNbBVPrimTests = 0; - mPairs.Reset(); - return true; - } - } - } - - // Here, hulls collide - cache.HullTest = false; -#endif // __MESHMERIZER_H__ - // Checkings if(!Setup(cache.Model0->GetMeshInterface(), cache.Model1->GetMeshInterface())) return false; // Simple double-dispatch bool Status; - if(!cache.Model0->HasLeafNodes()) - { - if(cache.Model0->IsQuantized()) - { - const AABBQuantizedNoLeafTree* T0 = (const AABBQuantizedNoLeafTree*)cache.Model0->GetTree(); - const AABBQuantizedNoLeafTree* T1 = (const AABBQuantizedNoLeafTree*)cache.Model1->GetTree(); - Status = Collide(T0, T1, world0, world1, &cache); - } - else - { - const AABBNoLeafTree* T0 = (const AABBNoLeafTree*)cache.Model0->GetTree(); - const AABBNoLeafTree* T1 = (const AABBNoLeafTree*)cache.Model1->GetTree(); - Status = Collide(T0, T1, world0, world1, &cache); - } - } - else - { - if(cache.Model0->IsQuantized()) - { - const AABBQuantizedTree* T0 = (const AABBQuantizedTree*)cache.Model0->GetTree(); - const AABBQuantizedTree* T1 = (const AABBQuantizedTree*)cache.Model1->GetTree(); - Status = Collide(T0, T1, world0, world1, &cache); - } - else - { - const AABBCollisionTree* T0 = (const AABBCollisionTree*)cache.Model0->GetTree(); - const AABBCollisionTree* T1 = (const AABBCollisionTree*)cache.Model1->GetTree(); - Status = Collide(T0, T1, world0, world1, &cache); - } - } - -#ifdef __MESHMERIZER_H__ - if(Status) - { - // Reset counter as long as overlap occurs - if(GetContactStatus()) cache.ResetCountDown(); - - // Enable hull test again when counter reaches zero - cache.CountDown--; - if(!cache.CountDown) - { - cache.ResetCountDown(); - cache.HullTest = true; - } - } -#endif + const AABBNoLeafTree* T0 = (const AABBNoLeafTree*)cache.Model0->GetTree(); + const AABBNoLeafTree* T1 = (const AABBNoLeafTree*)cache.Model1->GetTree(); + Status = Collide(T0, T1, world0, world1, &cache); return Status; } @@ -351,82 +242,6 @@ bool AABBTreeCollider::Collide(const AABBNoLeafTree* tree0, const AABBNoLeafTree return true; } -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/** - * Collision query for quantized AABB trees. - * \param tree0 [in] AABB tree from first object - * \param tree1 [in] AABB tree from second object - * \param world0 [in] world matrix for first object - * \param world1 [in] world matrix for second object - * \param cache [in/out] cache for a pair of previously colliding primitives - * \return true if success - * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only. - */ -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -bool AABBTreeCollider::Collide(const AABBQuantizedTree* tree0, const AABBQuantizedTree* tree1, const Matrix4x4* world0, const Matrix4x4* world1, Pair* cache) -{ - // Init collision query - InitQuery(world0, world1); - - // Check previous state - if(CheckTemporalCoherence(cache)) return true; - - // Setup dequantization coeffs - mCenterCoeff0 = tree0->mCenterCoeff; - mExtentsCoeff0 = tree0->mExtentsCoeff; - mCenterCoeff1 = tree1->mCenterCoeff; - mExtentsCoeff1 = tree1->mExtentsCoeff; - - // Dequantize box A - const AABBQuantizedNode* N0 = tree0->GetNodes(); - const Point a(float(N0->mAABB.mExtents[0]) * mExtentsCoeff0.x, float(N0->mAABB.mExtents[1]) * mExtentsCoeff0.y, float(N0->mAABB.mExtents[2]) * mExtentsCoeff0.z); - const Point Pa(float(N0->mAABB.mCenter[0]) * mCenterCoeff0.x, float(N0->mAABB.mCenter[1]) * mCenterCoeff0.y, float(N0->mAABB.mCenter[2]) * mCenterCoeff0.z); - // Dequantize box B - const AABBQuantizedNode* N1 = tree1->GetNodes(); - const Point b(float(N1->mAABB.mExtents[0]) * mExtentsCoeff1.x, float(N1->mAABB.mExtents[1]) * mExtentsCoeff1.y, float(N1->mAABB.mExtents[2]) * mExtentsCoeff1.z); - const Point Pb(float(N1->mAABB.mCenter[0]) * mCenterCoeff1.x, float(N1->mAABB.mCenter[1]) * mCenterCoeff1.y, float(N1->mAABB.mCenter[2]) * mCenterCoeff1.z); - - // Perform collision query - _Collide(N0, N1, a, Pa, b, Pb); - - UPDATE_CACHE - - return true; -} - -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/** - * Collision query for quantized no-leaf AABB trees. - * \param tree0 [in] AABB tree from first object - * \param tree1 [in] AABB tree from second object - * \param world0 [in] world matrix for first object - * \param world1 [in] world matrix for second object - * \param cache [in/out] cache for a pair of previously colliding primitives - * \return true if success - * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only. - */ -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -bool AABBTreeCollider::Collide(const AABBQuantizedNoLeafTree* tree0, const AABBQuantizedNoLeafTree* tree1, const Matrix4x4* world0, const Matrix4x4* world1, Pair* cache) -{ - // Init collision query - InitQuery(world0, world1); - - // Check previous state - if(CheckTemporalCoherence(cache)) return true; - - // Setup dequantization coeffs - mCenterCoeff0 = tree0->mCenterCoeff; - mExtentsCoeff0 = tree0->mExtentsCoeff; - mCenterCoeff1 = tree1->mCenterCoeff; - mExtentsCoeff1 = tree1->mExtentsCoeff; - - // Perform collision query - _Collide(tree0->GetNodes(), tree1->GetNodes()); - - UPDATE_CACHE - - return true; -} /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Standard trees @@ -530,10 +345,8 @@ void AABBTreeCollider::PrimTest(udword id0, udword id1) // Request vertices from the app VertexPointers VP0; VertexPointers VP1; - ConversionArea VC0; - ConversionArea VC1; - mIMesh0->GetTriangle(VP0, id0, VC0); - mIMesh1->GetTriangle(VP1, id1, VC1); + mIMesh0->GetTriangle(VP0, id0); + mIMesh1->GetTriangle(VP1, id1); // Transform from space 1 to space 0 Point u0,u1,u2; @@ -561,8 +374,7 @@ inline_ void AABBTreeCollider::PrimTestTriIndex(udword id1) { // Request vertices from the app VertexPointers VP; - ConversionArea VC; - mIMesh1->GetTriangle(VP, id1, VC); + mIMesh1->GetTriangle(VP, id1); // Perform triangle-triangle overlap test if(TriTriOverlap(mLeafVerts[0], mLeafVerts[1], mLeafVerts[2], *VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2])) @@ -584,8 +396,7 @@ inline_ void AABBTreeCollider::PrimTestIndexTri(udword id0) { // Request vertices from the app VertexPointers VP; - ConversionArea VC; - mIMesh0->GetTriangle(VP, id0, VC); + mIMesh0->GetTriangle(VP, id0); // Perform triangle-triangle overlap test if(TriTriOverlap(mLeafVerts[0], mLeafVerts[1], mLeafVerts[2], *VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2])) @@ -645,7 +456,7 @@ void AABBTreeCollider::_CollideBoxTri(const AABBNoLeafNode* b) #define FETCH_LEAF(prim_index, imesh, rot, trans) \ mLeafIndex = prim_index; \ /* Request vertices from the app */ \ - VertexPointers VP; ConversionArea VC; imesh->GetTriangle(VP, prim_index, VC); \ + VertexPointers VP; imesh->GetTriangle(VP, prim_index); \ /* Transform them in a common space */ \ TransformPoint(mLeafVerts[0], *VP.Vertex[0], rot, trans); \ TransformPoint(mLeafVerts[1], *VP.Vertex[1], rot, trans); \ @@ -738,210 +549,4 @@ void AABBTreeCollider::_Collide(const AABBNoLeafNode* a, const AABBNoLeafNode* b } } -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -// Quantized trees -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/** - * Recursive collision query for quantized AABB trees. - * \param b0 [in] collision node from first tree - * \param b1 [in] collision node from second tree - * \param a [in] extent from box A - * \param Pa [in] center from box A - * \param b [in] extent from box B - * \param Pb [in] center from box B - */ -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void AABBTreeCollider::_Collide(const AABBQuantizedNode* b0, const AABBQuantizedNode* b1, const Point& a, const Point& Pa, const Point& b, const Point& Pb) -{ - // Perform BV-BV overlap test - if(!BoxBoxOverlap(a, Pa, b, Pb)) return; - - if(b0->IsLeaf() && b1->IsLeaf()) { PrimTest(b0->GetPrimitive(), b1->GetPrimitive()); return; } - - if(b1->IsLeaf() || (!b0->IsLeaf() && (b0->GetSize() > b1->GetSize()))) - { - // Dequantize box - const QuantizedAABB* Box = &b0->GetNeg()->mAABB; - const Point negPa(float(Box->mCenter[0]) * mCenterCoeff0.x, float(Box->mCenter[1]) * mCenterCoeff0.y, float(Box->mCenter[2]) * mCenterCoeff0.z); - const Point nega(float(Box->mExtents[0]) * mExtentsCoeff0.x, float(Box->mExtents[1]) * mExtentsCoeff0.y, float(Box->mExtents[2]) * mExtentsCoeff0.z); - _Collide(b0->GetNeg(), b1, nega, negPa, b, Pb); - - if(ContactFound()) return; - - // Dequantize box - Box = &b0->GetPos()->mAABB; - const Point posPa(float(Box->mCenter[0]) * mCenterCoeff0.x, float(Box->mCenter[1]) * mCenterCoeff0.y, float(Box->mCenter[2]) * mCenterCoeff0.z); - const Point posa(float(Box->mExtents[0]) * mExtentsCoeff0.x, float(Box->mExtents[1]) * mExtentsCoeff0.y, float(Box->mExtents[2]) * mExtentsCoeff0.z); - _Collide(b0->GetPos(), b1, posa, posPa, b, Pb); - } - else - { - // Dequantize box - const QuantizedAABB* Box = &b1->GetNeg()->mAABB; - const Point negPb(float(Box->mCenter[0]) * mCenterCoeff1.x, float(Box->mCenter[1]) * mCenterCoeff1.y, float(Box->mCenter[2]) * mCenterCoeff1.z); - const Point negb(float(Box->mExtents[0]) * mExtentsCoeff1.x, float(Box->mExtents[1]) * mExtentsCoeff1.y, float(Box->mExtents[2]) * mExtentsCoeff1.z); - _Collide(b0, b1->GetNeg(), a, Pa, negb, negPb); - - if(ContactFound()) return; - - // Dequantize box - Box = &b1->GetPos()->mAABB; - const Point posPb(float(Box->mCenter[0]) * mCenterCoeff1.x, float(Box->mCenter[1]) * mCenterCoeff1.y, float(Box->mCenter[2]) * mCenterCoeff1.z); - const Point posb(float(Box->mExtents[0]) * mExtentsCoeff1.x, float(Box->mExtents[1]) * mExtentsCoeff1.y, float(Box->mExtents[2]) * mExtentsCoeff1.z); - _Collide(b0, b1->GetPos(), a, Pa, posb, posPb); - } -} - -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -// Quantized no-leaf trees -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/** - * Recursive collision of a leaf node from A and a quantized branch from B. - * \param leaf [in] leaf triangle from first tree - * \param b [in] collision node from second tree - */ -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void AABBTreeCollider::_CollideTriBox(const AABBQuantizedNoLeafNode* b) -{ - // Dequantize box - const QuantizedAABB* bb = &b->mAABB; - const Point Pb(float(bb->mCenter[0]) * mCenterCoeff1.x, float(bb->mCenter[1]) * mCenterCoeff1.y, float(bb->mCenter[2]) * mCenterCoeff1.z); - const Point eb(float(bb->mExtents[0]) * mExtentsCoeff1.x, float(bb->mExtents[1]) * mExtentsCoeff1.y, float(bb->mExtents[2]) * mExtentsCoeff1.z); - - // Perform triangle-box overlap test - if(!TriBoxOverlap(Pb, eb)) return; - - if(b->HasPosLeaf()) PrimTestTriIndex(b->GetPosPrimitive()); - else _CollideTriBox(b->GetPos()); - - if(ContactFound()) return; - - if(b->HasNegLeaf()) PrimTestTriIndex(b->GetNegPrimitive()); - else _CollideTriBox(b->GetNeg()); -} - -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/** - * Recursive collision of a leaf node from B and a quantized branch from A. - * \param b [in] collision node from first tree - * \param leaf [in] leaf triangle from second tree - */ -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void AABBTreeCollider::_CollideBoxTri(const AABBQuantizedNoLeafNode* b) -{ - // Dequantize box - const QuantizedAABB* bb = &b->mAABB; - const Point Pa(float(bb->mCenter[0]) * mCenterCoeff0.x, float(bb->mCenter[1]) * mCenterCoeff0.y, float(bb->mCenter[2]) * mCenterCoeff0.z); - const Point ea(float(bb->mExtents[0]) * mExtentsCoeff0.x, float(bb->mExtents[1]) * mExtentsCoeff0.y, float(bb->mExtents[2]) * mExtentsCoeff0.z); - - // Perform triangle-box overlap test - if(!TriBoxOverlap(Pa, ea)) return; - - if(b->HasPosLeaf()) PrimTestIndexTri(b->GetPosPrimitive()); - else _CollideBoxTri(b->GetPos()); - - if(ContactFound()) return; - - if(b->HasNegLeaf()) PrimTestIndexTri(b->GetNegPrimitive()); - else _CollideBoxTri(b->GetNeg()); -} - -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/** - * Recursive collision query for quantized no-leaf AABB trees. - * \param a [in] collision node from first tree - * \param b [in] collision node from second tree - */ -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void AABBTreeCollider::_Collide(const AABBQuantizedNoLeafNode* a, const AABBQuantizedNoLeafNode* b) -{ - // Dequantize box A - const QuantizedAABB* ab = &a->mAABB; - const Point Pa(float(ab->mCenter[0]) * mCenterCoeff0.x, float(ab->mCenter[1]) * mCenterCoeff0.y, float(ab->mCenter[2]) * mCenterCoeff0.z); - const Point ea(float(ab->mExtents[0]) * mExtentsCoeff0.x, float(ab->mExtents[1]) * mExtentsCoeff0.y, float(ab->mExtents[2]) * mExtentsCoeff0.z); - // Dequantize box B - const QuantizedAABB* bb = &b->mAABB; - const Point Pb(float(bb->mCenter[0]) * mCenterCoeff1.x, float(bb->mCenter[1]) * mCenterCoeff1.y, float(bb->mCenter[2]) * mCenterCoeff1.z); - const Point eb(float(bb->mExtents[0]) * mExtentsCoeff1.x, float(bb->mExtents[1]) * mExtentsCoeff1.y, float(bb->mExtents[2]) * mExtentsCoeff1.z); - - // Perform BV-BV overlap test - if(!BoxBoxOverlap(ea, Pa, eb, Pb)) return; - - // Catch leaf status - BOOL BHasPosLeaf = b->HasPosLeaf(); - BOOL BHasNegLeaf = b->HasNegLeaf(); - - if(a->HasPosLeaf()) - { - FETCH_LEAF(a->GetPosPrimitive(), mIMesh0, mR0to1, mT0to1) - - if(BHasPosLeaf) PrimTestTriIndex(b->GetPosPrimitive()); - else _CollideTriBox(b->GetPos()); - - if(ContactFound()) return; - - if(BHasNegLeaf) PrimTestTriIndex(b->GetNegPrimitive()); - else _CollideTriBox(b->GetNeg()); - } - else - { - if(BHasPosLeaf) - { - FETCH_LEAF(b->GetPosPrimitive(), mIMesh1, mR1to0, mT1to0) - - _CollideBoxTri(a->GetPos()); - } - else _Collide(a->GetPos(), b->GetPos()); - - if(ContactFound()) return; - - if(BHasNegLeaf) - { - FETCH_LEAF(b->GetNegPrimitive(), mIMesh1, mR1to0, mT1to0) - - _CollideBoxTri(a->GetPos()); - } - else _Collide(a->GetPos(), b->GetNeg()); - } - - if(ContactFound()) return; - - if(a->HasNegLeaf()) - { - FETCH_LEAF(a->GetNegPrimitive(), mIMesh0, mR0to1, mT0to1) - - if(BHasPosLeaf) PrimTestTriIndex(b->GetPosPrimitive()); - else _CollideTriBox(b->GetPos()); - - if(ContactFound()) return; - - if(BHasNegLeaf) PrimTestTriIndex(b->GetNegPrimitive()); - else _CollideTriBox(b->GetNeg()); - } - else - { - if(BHasPosLeaf) - { - // ### That leaf has possibly already been fetched - FETCH_LEAF(b->GetPosPrimitive(), mIMesh1, mR1to0, mT1to0) - - _CollideBoxTri(a->GetNeg()); - } - else _Collide(a->GetNeg(), b->GetPos()); - - if(ContactFound()) return; - - if(BHasNegLeaf) - { - // ### That leaf has possibly already been fetched - FETCH_LEAF(b->GetNegPrimitive(), mIMesh1, mR1to0, mT1to0) - - _CollideBoxTri(a->GetNeg()); - } - else _Collide(a->GetNeg(), b->GetNeg()); - } -} diff --git a/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_TreeCollider.h b/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_TreeCollider.h index 1e943a4b..5a5afede 100644 --- a/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_TreeCollider.h +++ b/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_TreeCollider.h @@ -41,31 +41,12 @@ Model1 = null; id0 = 0; id1 = 1; -#ifdef __MESHMERIZER_H__ // Collision hulls only supported within ICE ! - HullTest = true; - SepVector.pid = 0; - SepVector.qid = 0; - SepVector.SV = Point(1.0f, 0.0f, 0.0f); -#endif // __MESHMERIZER_H__ } -#ifdef __MESHMERIZER_H__ // Collision hulls only supported within ICE ! - inline_ void ResetCountDown() - { - CountDown = 50; - } -#else void ResetCountDown(){}; -#endif // __MESHMERIZER_H__ const Model* Model0; //!< Model for first object const Model* Model1; //!< Model for second object - -#ifdef __MESHMERIZER_H__ // Collision hulls only supported within ICE ! - SVCache SepVector; - udword CountDown; - bool HullTest; -#endif // __MESHMERIZER_H__ }; class OPCODE_API AABBTreeCollider : public Collider @@ -95,8 +76,6 @@ // Collision queries bool Collide(const AABBCollisionTree* tree0, const AABBCollisionTree* tree1, const Matrix4x4* world0=null, const Matrix4x4* world1=null, Pair* cache=null); bool Collide(const AABBNoLeafTree* tree0, const AABBNoLeafTree* tree1, const Matrix4x4* world0=null, const Matrix4x4* world1=null, Pair* cache=null); - bool Collide(const AABBQuantizedTree* tree0, const AABBQuantizedTree* tree1, const Matrix4x4* world0=null, const Matrix4x4* world1=null, Pair* cache=null); - bool Collide(const AABBQuantizedNoLeafTree* tree0, const AABBQuantizedNoLeafTree* tree1, const Matrix4x4* world0=null, const Matrix4x4* world1=null, Pair* cache=null); // Settings /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -210,16 +189,10 @@ // Standard AABB trees void _Collide(const AABBCollisionNode* b0, const AABBCollisionNode* b1); - // Quantized AABB trees - void _Collide(const AABBQuantizedNode* b0, const AABBQuantizedNode* b1, const Point& a, const Point& Pa, const Point& b, const Point& Pb); // No-leaf AABB trees void _CollideTriBox(const AABBNoLeafNode* b); void _CollideBoxTri(const AABBNoLeafNode* b); void _Collide(const AABBNoLeafNode* a, const AABBNoLeafNode* b); - // Quantized no-leaf AABB trees - void _CollideTriBox(const AABBQuantizedNoLeafNode* b); - void _CollideBoxTri(const AABBQuantizedNoLeafNode* b); - void _Collide(const AABBQuantizedNoLeafNode* a, const AABBQuantizedNoLeafNode* b); // Overlap tests void PrimTest(udword id0, udword id1); inline_ void PrimTestTriIndex(udword id1); diff --git a/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_TriBoxOverlap.h b/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_TriBoxOverlap.h index b3a9bdee..4f2026ea 100644 --- a/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_TriBoxOverlap.h +++ b/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_TriBoxOverlap.h @@ -11,13 +11,24 @@ inline_ BOOL planeBoxOverlap(const Point& normal, const float d, const Point& maxbox) { Point vmin, vmax; - for(udword q=0;q<=2;q++) + for(int q = 0; q <= 2; q++) { - if(normal[q]>0.0f) { vmin[q]=-maxbox[q]; vmax[q]=maxbox[q]; } - else { vmin[q]=maxbox[q]; vmax[q]=-maxbox[q]; } + if(normal[q] > 0.0f) + { + vmin[q] = -maxbox[q]; + vmax[q] = maxbox[q]; + } + else + { + vmin[q] = maxbox[q]; + vmax[q] = -maxbox[q]; + } } - if((normal|vmin)+d>0.0f) return FALSE; - if((normal|vmax)+d>=0.0f) return TRUE; + + if((normal | vmin) + d > 0.0f) + return FALSE; + if((normal | vmax) + d >= 0.0f) + return TRUE; return FALSE; } @@ -130,54 +141,49 @@ inline_ BOOL AABBTreeCollider::TriBoxOverlap(const Point& center, const Point& e // move everything so that the boxcenter is in (0,0,0) Point v0, v1, v2; - v0.x = mLeafVerts[0].x - center.x; - v1.x = mLeafVerts[1].x - center.x; - v2.x = mLeafVerts[2].x - center.x; +#if defined(__AVX__) + v0 = mLeafVerts[0] - center; + v1 = mLeafVerts[1] - center; + v2 = mLeafVerts[2] - center; - // First, test overlap in the {x,y,z}-directions -#ifdef OPC_USE_FCOMI - // find min, max of the triangle in x-direction, and test for overlap in X - if(FCMin3(v0.x, v1.x, v2.x)>extents.x) return FALSE; - if(FCMax3(v0.x, v1.x, v2.x)<-extents.x) return FALSE; + // First, test overlap in the {x,y,z}-directions + // find min, max of the triangle in x-direction, and test for overlap in X + if (!inExtent(v0.x, v1.x, v2.x, extents.x)) + return FALSE; - // same for Y - v0.y = mLeafVerts[0].y - center.y; - v1.y = mLeafVerts[1].y - center.y; - v2.y = mLeafVerts[2].y - center.y; - - if(FCMin3(v0.y, v1.y, v2.y)>extents.y) return FALSE; - if(FCMax3(v0.y, v1.y, v2.y)<-extents.y) return FALSE; - - // same for Z - v0.z = mLeafVerts[0].z - center.z; - v1.z = mLeafVerts[1].z - center.z; - v2.z = mLeafVerts[2].z - center.z; - - if(FCMin3(v0.z, v1.z, v2.z)>extents.z) return FALSE; - if(FCMax3(v0.z, v1.z, v2.z)<-extents.z) return FALSE; + if (!inExtent(v0.y, v1.y, v2.y, extents.y)) + return FALSE; + + if (!inExtent(v0.z, v1.z, v2.z, extents.z)) + return FALSE; #else - float min,max; - // Find min, max of the triangle in x-direction, and test for overlap in X - FINDMINMAX(v0.x, v1.x, v2.x, min, max); - if(min>extents.x || max<-extents.x) return FALSE; + v0.x = mLeafVerts[0].x - center.x; + v1.x = mLeafVerts[1].x - center.x; + v2.x = mLeafVerts[2].x - center.x; - // Same for Y - v0.y = mLeafVerts[0].y - center.y; - v1.y = mLeafVerts[1].y - center.y; - v2.y = mLeafVerts[2].y - center.y; + // First, test overlap in the {x,y,z}-directions + // find min, max of the triangle in x-direction, and test for overlap in X + if (!inExtent(v0.x, v1.x, v2.x, extents.x)) + return FALSE; - FINDMINMAX(v0.y, v1.y, v2.y, min, max); - if(min>extents.y || max<-extents.y) return FALSE; + // same for Y + v0.y = mLeafVerts[0].y - center.y; + v1.y = mLeafVerts[1].y - center.y; + v2.y = mLeafVerts[2].y - center.y; - // Same for Z - v0.z = mLeafVerts[0].z - center.z; - v1.z = mLeafVerts[1].z - center.z; - v2.z = mLeafVerts[2].z - center.z; + if (!inExtent(v0.y, v1.y, v2.y, extents.y)) + return FALSE; + + // same for Z + v0.z = mLeafVerts[0].z - center.z; + v1.z = mLeafVerts[1].z - center.z; + v2.z = mLeafVerts[2].z - center.z; + + if (!inExtent(v0.z, v1.z, v2.z, extents.z)) + return FALSE; - FINDMINMAX(v0.z, v1.z, v2.z, min, max); - if(min>extents.z || max<-extents.z) return FALSE; #endif - // 2) Test if the box intersects the plane of the triangle + // 2) Test if the box intersects the plane of the triangle // compute plane equation of triangle: normal*x+d=0 // ### could be precomputed since we use the same leaf triangle several times const Point e0 = v1 - v0; @@ -217,28 +223,14 @@ inline_ BOOL OBBCollider::TriBoxOverlap() // Box center is already in (0,0,0) // First, test overlap in the {x,y,z}-directions -#ifdef OPC_USE_FCOMI // find min, max of the triangle in x-direction, and test for overlap in X - if(FCMin3(v0.x, v1.x, v2.x)>mBoxExtents.x) return FALSE; - if(FCMax3(v0.x, v1.x, v2.x)<-mBoxExtents.x) return FALSE; + if(!inExtent(v0.x, v1.x, v2.x, mBoxExtents.x)) + return FALSE; + if(!inExtent(v0.y, v1.y, v2.y, mBoxExtents.y)) + return FALSE; + if(!inExtent(v0.z, v1.z, v2.z, mBoxExtents.z)) + return FALSE; - if(FCMin3(v0.y, v1.y, v2.y)>mBoxExtents.y) return FALSE; - if(FCMax3(v0.y, v1.y, v2.y)<-mBoxExtents.y) return FALSE; - - if(FCMin3(v0.z, v1.z, v2.z)>mBoxExtents.z) return FALSE; - if(FCMax3(v0.z, v1.z, v2.z)<-mBoxExtents.z) return FALSE; -#else - float min,max; - // Find min, max of the triangle in x-direction, and test for overlap in X - FINDMINMAX(v0.x, v1.x, v2.x, min, max); - if(min>mBoxExtents.x || max<-mBoxExtents.x) return FALSE; - - FINDMINMAX(v0.y, v1.y, v2.y, min, max); - if(min>mBoxExtents.y || max<-mBoxExtents.y) return FALSE; - - FINDMINMAX(v0.z, v1.z, v2.z, min, max); - if(min>mBoxExtents.z || max<-mBoxExtents.z) return FALSE; -#endif // 2) Test if the box intersects the plane of the triangle // compute plane equation of triangle: normal*x+d=0 // ### could be precomputed since we use the same leaf triangle several times @@ -280,7 +272,7 @@ inline_ BOOL AABBCollider::TriBoxOverlap() v2.x = mLeafVerts[2].x - center.x; // First, test overlap in the {x,y,z}-directions -#ifdef OPC_USE_FCOMI + // find min, max of the triangle in x-direction, and test for overlap in X if(FCMin3(v0.x, v1.x, v2.x)>extents.x) return FALSE; if(FCMax3(v0.x, v1.x, v2.x)<-extents.x) return FALSE; @@ -300,28 +292,6 @@ inline_ BOOL AABBCollider::TriBoxOverlap() if(FCMin3(v0.z, v1.z, v2.z)>extents.z) return FALSE; if(FCMax3(v0.z, v1.z, v2.z)<-extents.z) return FALSE; -#else - float min,max; - // Find min, max of the triangle in x-direction, and test for overlap in X - FINDMINMAX(v0.x, v1.x, v2.x, min, max); - if(min>extents.x || max<-extents.x) return FALSE; - - // Same for Y - v0.y = mLeafVerts[0].y - center.y; - v1.y = mLeafVerts[1].y - center.y; - v2.y = mLeafVerts[2].y - center.y; - - FINDMINMAX(v0.y, v1.y, v2.y, min, max); - if(min>extents.y || max<-extents.y) return FALSE; - - // Same for Z - v0.z = mLeafVerts[0].z - center.z; - v1.z = mLeafVerts[1].z - center.z; - v2.z = mLeafVerts[2].z - center.z; - - FINDMINMAX(v0.z, v1.z, v2.z, min, max); - if(min>extents.z || max<-extents.z) return FALSE; -#endif // 2) Test if the box intersects the plane of the triangle // compute plane equation of triangle: normal*x+d=0 // ### could be precomputed since we use the same leaf triangle several times diff --git a/trunk/unmanaged/ubODE-OpenSim/OPENSIM-README.txt b/trunk/unmanaged/ubODE-OpenSim/OPENSIM-README.txt index 61e24a90..1d0beed1 100644 --- a/trunk/unmanaged/ubODE-OpenSim/OPENSIM-README.txt +++ b/trunk/unmanaged/ubODE-OpenSim/OPENSIM-README.txt @@ -1,5 +1,5 @@ OPENSIM README -this is version ODE-OpenSim.0.13.3 for ubODE +this is version ODE-OpenSim.0.13.4 for ubODE WARNING, do not forget to rename the dlls, they are still created named as *ode* read below The ODE code in this repository correspondes to ODE release 0.13.1 r1902 with selected adictions from more recent vrsions and modifications by opensim diff --git a/trunk/unmanaged/ubODE-OpenSim/include/ode/collision.h b/trunk/unmanaged/ubODE-OpenSim/include/ode/collision.h index cbaac042..18a4a832 100644 --- a/trunk/unmanaged/ubODE-OpenSim/include/ode/collision.h +++ b/trunk/unmanaged/ubODE-OpenSim/include/ode/collision.h @@ -207,8 +207,7 @@ ODE_API void dGeomCopyPosition (dGeomID geom, dVector3 pos); * @sa dBodyGetRotation * @ingroup collide */ -ODE_API const dReal * dGeomGetRotation (dGeomID geom); - +ODE_API const dReal* dGeomGetRotation (dGeomID geom); /** * @brief Get the rotation matrix of a placeable geom. @@ -225,7 +224,6 @@ ODE_API const dReal * dGeomGetRotation (dGeomID geom); */ ODE_API void dGeomCopyRotation(dGeomID geom, dMatrix3 R); - /** * @brief Get the rotation quaternion of a placeable geom. * diff --git a/trunk/unmanaged/ubODE-OpenSim/include/ode/common.h b/trunk/unmanaged/ubODE-OpenSim/include/ode/common.h index 716569c0..57e1b2ac 100644 --- a/trunk/unmanaged/ubODE-OpenSim/include/ode/common.h +++ b/trunk/unmanaged/ubODE-OpenSim/include/ode/common.h @@ -69,8 +69,6 @@ typedef double dReal; #endif /* Detect if we've got both trimesh engines enabled. */ - - /* * Define a type for indices, either 16 or 32 bit, based on build option * TODO: Currently GIMPACT only supports 32 bit indices. diff --git a/trunk/unmanaged/ubODE-OpenSim/include/ode/odemath.h b/trunk/unmanaged/ubODE-OpenSim/include/ode/odemath.h index 3531bfa6..0f5da307 100644 --- a/trunk/unmanaged/ubODE-OpenSim/include/ode/odemath.h +++ b/trunk/unmanaged/ubODE-OpenSim/include/ode/odemath.h @@ -169,7 +169,7 @@ ODE_PURE_INLINE void dAddVectors3r4(dReal *res, const dReal *a, const dReal *b) #endif #if defined(__AVX__) -ODE_PURE_INLINE void dSubtractVector3(dReal *res, const dReal *a) +ODE_PURE_INLINE void dSubtractVectors3(dReal *res, const dReal *a) { __m128 ma, mb; ma = _mm_loadu_ps(res); @@ -208,7 +208,7 @@ ODE_PURE_INLINE void dSubtractVectors3(dReal *res, const dReal *a, const dReal * #endif #if defined(__AVX__) -ODE_PURE_INLINE void dSubtractVector3r4(dReal *res, const dReal *a) +ODE_PURE_INLINE void dSubtractVectors3r4(dReal *res, const dReal *a) { __m128 ma, mb; ma = _mm_loadu_ps(res); @@ -218,7 +218,7 @@ ODE_PURE_INLINE void dSubtractVector3r4(dReal *res, const dReal *a) _mm_storeu_ps(res, ma); } #else -ODE_PURE_INLINE void dSubtractVector3r4(dReal *res, const dReal *a) +ODE_PURE_INLINE void dSubtractVectors3r4(dReal *res, const dReal *a) { res[0] -= a[0]; res[1] -= a[1]; @@ -245,20 +245,6 @@ ODE_PURE_INLINE void dSubtractVectors3r4(dReal *res, const dReal *a, const dReal } #endif -#if defined(__AVX__) -ODE_PURE_INLINE void dSubNoAliaseVectors3(dReal *res, const dReal *a, const dReal *b) -{ - dSubtractVectors3(res, a, b); -} -#else -ODE_PURE_INLINE void dSubNoAliaseVectors3(dReal *res, const dReal *a, const dReal *b) -{ - res[0] = a[0] - b[0]; - res[1] = a[1] - b[1]; - res[2] = a[2] - b[2]; -} -#endif - #if defined(__AVX__) ODE_PURE_INLINE void dAddScaledVector3(dReal *res, const dReal *a, const dReal scale) { @@ -540,10 +526,10 @@ ODE_PURE_INLINE void dCopyVector3r4(dReal *res, const dReal *a) #if defined(__AVX__) ODE_PURE_INLINE void dFabsVector3(dReal *res) { - __m128 ma, sign; + const __m128 sign = _mm_set1_ps(-0.0f); + __m128 ma; ma = _mm_loadu_ps(res); - sign = _mm_set1_ps(-0.0f); ma = _mm_andnot_ps(sign, ma); store3f(res, ma); @@ -560,10 +546,10 @@ ODE_PURE_INLINE void dFabsVector3(dReal *res) #if defined(__AVX__) ODE_PURE_INLINE void dFabsVector3r4(dReal *res) { - __m128 ma, sign; + const __m128 sign = _mm_set1_ps(-0.0f); + __m128 ma; ma = _mm_loadu_ps(res); - sign = _mm_set1_ps(-0.0f); ma = _mm_andnot_ps(sign, ma); _mm_storeu_ps(res, ma); @@ -580,10 +566,10 @@ ODE_PURE_INLINE void dFabsVector3r4(dReal *res) #if defined(__AVX__) ODE_PURE_INLINE void dFabsVector3(dReal *res, const dReal *a) { - __m128 ma, sign; + const __m128 sign = _mm_set1_ps(-0.0f); + __m128 ma; ma = _mm_loadu_ps(a); - sign = _mm_set1_ps(-0.0f); ma = _mm_andnot_ps(sign, ma); store3f(res, ma); @@ -600,10 +586,10 @@ ODE_PURE_INLINE void dFabsVector3(dReal *res, const dReal *a) #if defined(__AVX__) ODE_PURE_INLINE void dFabsVector3r4(dReal *res, const dReal *a) { - __m128 ma, sign; + const __m128 sign = _mm_set1_ps(-0.0f); + __m128 ma; ma = _mm_loadu_ps(a); - sign = _mm_set1_ps(-0.0f); ma = _mm_andnot_ps(sign, ma); _mm_storeu_ps(res, ma); @@ -918,14 +904,15 @@ ODE_PURE_INLINE dReal dCalcPointDepth3(const dReal *test_p, const dReal *plane_p #if defined(__AVX__) ODE_PURE_INLINE dReal dCalcPointPlaneDistance(const dVector3 point, const dVector4 plane) { - __m128 mp, mn, mc; + __m128 mp, mn; mp = _mm_loadu_ps(plane); mn = _mm_loadu_ps(point); - mc = _mm_set1_ps(1.0); - mn = _mm_blend_ps(mn, mc, 8); + mn = _mm_dp_ps(mp, mn, 0x71); + + mp = _mm_shuffle_ps(mp, mp, _MM_SHUFFLE(0, 0, 0, 3)); + mp = _mm_add_ps(mp, mn); - mp = _mm_dp_ps(mp, mn, 0xf1); return (dReal)_mm_cvtss_f32(mp); } #else @@ -952,6 +939,137 @@ ODE_PURE_INLINE dReal dCalcVectorDot3(const dReal *a, const dReal *b) } #endif +#if defined(__AVX__) +ODE_PURE_INLINE void dMinVector3r4(dReal *min, const dReal *a, const dReal *b) +{ + __m128 ma, mb; + ma = _mm_loadu_ps(a); + mb = _mm_loadu_ps(b); + ma = _mm_min_ps(ma, mb); + _mm_storeu_ps(min, ma); +} +#else +ODE_PURE_INLINE void dMinVector3r4(dReal *min, const dReal *a, const dReal *b) +{ + min[0] = a[0] < b[0] ? a[0] : b[0]; + min[1] = a[1] < b[1] ? a[1] : b[1]; + min[2] = a[2] < b[2] ? a[2] : b[2]; +} +#endif + +#if defined(__AVX__) +ODE_PURE_INLINE void dMinVector3r4(dReal *min, const dReal *b) +{ + __m128 ma, mb; + ma = _mm_loadu_ps(min); + mb = _mm_loadu_ps(b); + ma = _mm_min_ps(ma, mb); + _mm_storeu_ps(min, ma); +} +#else +ODE_PURE_INLINE void dMinVector3r4(dReal *min, const dReal *b) +{ + if (b[0] < min[0]) + min[0] = b[0]; + if (b[1] < min[1]) + min[1] = b[1]; + if (b[2] < min[2]) + min[2] = b[2]; +} +#endif + +#if defined(__AVX__) +ODE_PURE_INLINE void dMaxVector3r4(dReal *max, const dReal *a, const dReal *b) +{ + __m128 ma, mb; + ma = _mm_loadu_ps(a); + mb = _mm_loadu_ps(b); + ma = _mm_max_ps(ma, mb); + _mm_storeu_ps(max, ma); +} +#else +ODE_PURE_INLINE void dMaxVector3r4(dReal *max, const dReal *a, const dReal *b) +{ + max[0] = a[0] > b[0] ? a[0] : b[0]; + max[1] = a[1] > b[1] ? a[1] : b[1]; + max[2] = a[2] > b[2] ? a[2] : b[2]; +} +#endif + +#if defined(__AVX__) +ODE_PURE_INLINE void dMaxVectors3r4(dReal *max, const dReal *b) +{ + __m128 ma, mb; + ma = _mm_loadu_ps(max); + mb = _mm_loadu_ps(b); + ma = _mm_max_ps(ma, mb); + _mm_storeu_ps(max, ma); +} +#else +ODE_PURE_INLINE void dMaxVector3r4(dReal *max, const dReal *b) +{ + if (b[0] > max[0]) + max[0] = b[0]; + if (b[1] > max[1]) + max[1] = b[1]; + if (b[2] > max[2]) + max[2] = b[2]; +} +#endif + +#if defined(__AVX__) +ODE_PURE_INLINE void dMinMaxVectors3r4(dReal *min, dReal *max, const dReal *b) +{ + __m128 ma, mb, mc; + ma = _mm_loadu_ps(min); + mc = _mm_loadu_ps(b); + mb = _mm_loadu_ps(max); + + _mm_storeu_ps(min, _mm_min_ps(ma, mc)); + _mm_storeu_ps(max, _mm_max_ps(mb, mc)); +} +#else +ODE_PURE_INLINE void dMinMaxVectors3r4(dReal *min, dReal *max, const dReal *b) +{ + if (b[0] < min[0]) + min[0] = b[0]; + if (b[0] > max[0]) + max[0] = b[0]; + + if (b[1] < min[1]) + min[1] = b[1]; + if (b[1] > max[1]) + max[1] = b[1]; + + if (b[2] < min[2]) + min[2] = b[2]; + if (b[2] > max[2]) + max[2] = b[2]; +} +#endif + +#if defined(__AVX__) +ODE_PURE_INLINE void dAvgVectors3r4(dReal *avg, const dReal *a, const dReal *b) +{ + const __m128 half = _mm_set1_ps(0.5f); + __m128 ma, mb; + ma = _mm_loadu_ps(a); + mb = _mm_loadu_ps(b); + + ma = _mm_add_ps(ma, mb); + ma = _mm_mul_ps(ma, half); + + _mm_storeu_ps(avg, ma); +} +#else +ODE_PURE_INLINE void dAvgVectors3r4(dReal *avg, const dReal *a, const dReal *b) +{ + avg[0] = (a[0] + b[0]) * dReal(0.5); + avg[1] = (a[1] + b[1]) * dReal(0.5); + avg[2] = (a[2] + b[2]) * dReal(0.5); +} +#endif + #if defined(__AVX__) ODE_PURE_INLINE dReal dCalcVectorDot4(const dReal *a, const dReal *b) { @@ -1301,15 +1419,15 @@ ODE_PURE_INLINE void dCalcLerpVectors3(dReal *res, const dReal *a, const dReal * #if defined(__AVX__) ODE_PURE_INLINE void dCalcLerpVectors3r4(dReal *res, const dReal *a, const dReal *b, const dReal t) { - __m128 ma, mb, mc, t1, t2; + __m128 ma, mb, mc; ma = _mm_loadu_ps(a); mb = _mm_loadu_ps(b); mc = _mm_set1_ps(t); - t1 = _mm_sub_ps(mb, ma); - t2 = _mm_mul_ps(t1, mc); - mb = _mm_add_ps(ma, t2); + mb = _mm_sub_ps(mb, ma); + mb = _mm_mul_ps(mb, mc); + mb = _mm_add_ps(ma, mb); _mm_storeu_ps(res, mb); } @@ -1363,6 +1481,32 @@ ODE_PURE_INLINE void dMultVectors3r4(dReal *res, const dReal *a, const dReal *b) } #endif + +#if defined(__AVX__) +ODE_PURE_INLINE dReal dDotAbsVectors3r4(const dReal *a, const dReal *b) +{ + const __m128 sign = _mm_set1_ps(-0.0f); + __m128 ma, mb; + + mb = _mm_loadu_ps(a); + ma = _mm_loadu_ps(b); + ma = _mm_mul_ps(ma, mb); + + ma = _mm_andnot_ps(sign, ma); + + mb = _mm_hadd_ps(ma, ma); + ma = _mm_movehl_ps(ma, ma); + ma = _mm_add_ps(ma, mb); + + return (dReal)_mm_cvtss_f32(ma); +} +#else +ODE_PURE_INLINE dReal dDotAbsVectors3r4(const dReal *a, const dReal *b) +{ + return dFabs(a[0] * b[0]) + dFabs(a[1] * b[1]) + dFabs(a[2] * b[2]); +} +#endif + #if defined(__AVX__) ODE_PURE_INLINE void dMultVector3(dReal *res, const dReal *a) { @@ -1778,7 +1922,7 @@ ODE_PURE_INLINE dReal dInvertMatrix3(dReal *dst, const dReal *ma) #if defined(__AVX__) ODE_PURE_INLINE void dTransposetMatrix34(dReal *dst, const dReal *a) { - __m128 t0, t1, t2, m0, m1, m2, m3; + __m128 t0, t1, t2, m0, m2; t0 = _mm_loadu_ps(a); // a0 a1 a2 a3 t1 = _mm_loadu_ps(a + 4); // a4 a5 a6 a7 @@ -1812,6 +1956,28 @@ ODE_PURE_INLINE void dTransposetMatrix34(dReal *dst, const dReal *a) dst[10] = a[10]; } #endif + +#if defined(__AVX__) +ODE_PURE_INLINE bool avxBoxesOverlap(const dReal* acenter, const dReal* bcenter, const dReal* aext, const dReal* bext) +{ + const __m128 sign = _mm_set1_ps(-0.0f); + __m128 ma, mb, mc; + + ma = _mm_loadu_ps(acenter); + mb = _mm_loadu_ps(bcenter); + ma = _mm_sub_ps(ma, mb); + + ma = _mm_andnot_ps(sign, ma); + + mb = _mm_loadu_ps(aext); + mc = _mm_loadu_ps(bext); + mb = _mm_add_ps(mb, mc); + + ma = _mm_cmpgt_ps(ma, mb); + return ((_mm_movemask_ps(ma) & 0x07) == 0); +} +#endif + /* Include legacy macros here */ #include diff --git a/trunk/unmanaged/ubODE-OpenSim/include/ode/version.h b/trunk/unmanaged/ubODE-OpenSim/include/ode/version.h index 0b856a4f..827646c0 100644 --- a/trunk/unmanaged/ubODE-OpenSim/include/ode/version.h +++ b/trunk/unmanaged/ubODE-OpenSim/include/ode/version.h @@ -1,6 +1,6 @@ #ifndef _ODE_VERSION_H_ #define _ODE_VERSION_H_ -#define dODE_VERSION "OS0.13.3" +#define dODE_VERSION "OS0.13.4" #endif diff --git a/trunk/unmanaged/ubODE-OpenSim/ode/src/box.cpp b/trunk/unmanaged/ubODE-OpenSim/ode/src/box.cpp index 467215cb..fab71bda 100644 --- a/trunk/unmanaged/ubODE-OpenSim/ode/src/box.cpp +++ b/trunk/unmanaged/ubODE-OpenSim/ode/src/box.cpp @@ -106,22 +106,22 @@ void dGeomBoxGetLengths (dGeomID g, dVector3 result) dReal dGeomBoxPointDepth (dGeomID g, dReal x, dReal y, dReal z) { dUASSERT (g && g->type == dBoxClass,"argument not a box"); - g->recomputePosr(); - dxBox *b = (dxBox*) g; // Set p = (x,y,z) relative to box center - - dVector3 p,q; + dVector3 p; p[0] = x; p[1] = y; p[2] = z; - dSubtractVector3r4(p, b->final_posr->pos); + + dxPosR *box_posr = g->GetRecomputePosR(); + dSubtractVectors3r4(p, box_posr->pos); // Rotate p into box's coordinate frame, so we can // treat the OBB as an AABB - dMultiply1_331(q, b->final_posr->R, p); + dVector3 q; + dMultiply1_331(q, box_posr->R, p); dFabsVector3r4(p, q); - dSubtractVectors3r4(q, b->halfside, p); + dSubtractVectors3r4(q, ((dxBox*)g)->halfside, p); int i; dReal tmp; @@ -895,8 +895,10 @@ int dCollideBoxBox (dxGeom *o1, dxGeom *o2, int flags, dReal depth; dxBox *b1 = (dxBox*) o1; dxBox *b2 = (dxBox*) o2; - int num = dBoxBox (o1->final_posr->pos, o1->final_posr->R, b1->halfside, - o2->final_posr->pos, o2->final_posr->R, b2->halfside, + dxPosR *posr1 = o1->GetRecomputePosR(); + dxPosR *posr2 = o2->GetRecomputePosR(); + int num = dBoxBox (posr1->pos, posr1->R, b1->halfside, + posr2->pos, posr2->R, b2->halfside, normal, &depth, flags, contact, skip); for (int i=0; ipos, CONTACT(contact, skip)->pos, CONTACT(contact, 2 * skip)->pos); - dSubtractVector3r4(target->pos, p); + dSubtractVectors3r4(target->pos, p); target->depth = d4; ret++; } diff --git a/trunk/unmanaged/ubODE-OpenSim/ode/src/capsule.cpp b/trunk/unmanaged/ubODE-OpenSim/ode/src/capsule.cpp index db8cb688..54637f3b 100644 --- a/trunk/unmanaged/ubODE-OpenSim/ode/src/capsule.cpp +++ b/trunk/unmanaged/ubODE-OpenSim/ode/src/capsule.cpp @@ -53,26 +53,25 @@ dxGeom (space,1) updateZeroSizedFlag(!_radius/* || !_length -- zero length capsule is not a zero sized capsule*/); } - void dxCapsule::computeAABB() { - dVector3 vrot; - const dMatrix3& R = final_posr->R; - const dVector3& pos = final_posr->pos; + dxPosR *dpr = final_posr; + dVector3 extend; + dGetMatrixColumn3(extend, dpr->R, 2); + dScaleVector3r4(extend, halfLenZ); + dFabsVector3r4(extend); - dGetMatrixColumn3(vrot, R, 2); - dScaleVector3r4(vrot, halfLenZ); - dFabsVector3r4(vrot); + const dVector3& pos = dpr->pos; - dReal range = vrot[0] + radius; + dReal range = extend[0] + radius; aabb[0] = pos[0] - range; aabb[1] = pos[0] + range; - range = vrot[1] + radius; + range = extend[1] + radius; aabb[2] = pos[1] - range; aabb[3] = pos[1] + range; - range = vrot[2] + radius; + range = extend[2] + radius; aabb[4] = pos[2] - range; aabb[5] = pos[2] + range; } @@ -88,10 +87,9 @@ void dGeomCapsuleSetParams (dGeomID g, dReal radius, dReal length) { dUASSERT (g && g->type == dCapsuleClass,"argument not a ccylinder"); dAASSERT (radius >= 0 && length >= 0); - dxCapsule *c = (dxCapsule*) g; - c->radius = radius; - c->halfLenZ = REAL(0.5) * length; - c->updateZeroSizedFlag(!radius/* || !length -- zero length capsule is not a zero sized capsule*/); + ((dxCapsule*)g)->radius = radius; + ((dxCapsule*)g)->halfLenZ = REAL(0.5) * length; + ((dxCapsule*)g)->updateZeroSizedFlag(!radius/* || !length -- zero length capsule is not a zero sized capsule*/); dGeomMoved (g); } @@ -99,39 +97,37 @@ void dGeomCapsuleSetParams (dGeomID g, dReal radius, dReal length) void dGeomCapsuleGetParams (dGeomID g, dReal *radius, dReal *length) { dUASSERT (g && g->type == dCapsuleClass,"argument not a ccylinder"); - dxCapsule *c = (dxCapsule*) g; - *radius = c->radius; - *length = REAL(2.0) * c->halfLenZ; + *radius = ((dxCapsule*)g)->radius; + *length = REAL(2.0) * ((dxCapsule*)g)->halfLenZ; } dReal dGeomCapsulePointDepth (dGeomID g, dReal x, dReal y, dReal z) { dUASSERT (g && g->type == dCapsuleClass,"argument not a ccylinder"); - g->recomputePosr(); - dxCapsule *c = (dxCapsule*) g; - - const dReal* R = g->final_posr->R; - const dReal* pos = g->final_posr->pos; + dxPosR *dpr = g->GetRecomputePosR(); + dReal* pos = dpr->pos; + dVector3 a; a[0] = x; a[1] = y; a[2] = z; - dSubtractVector3r4(a, pos); + dSubtractVectors3r4(a, pos); dVector3 vrot; - dGetMatrixColumn3(vrot, R, 2); + dGetMatrixColumn3(vrot, dpr->R, 2); dReal beta = dCalcVectorDot3(a, vrot); - dReal lz2 = c->halfLenZ; + dReal lz2 = ((dxCapsule*)g)->halfLenZ; if (beta < -lz2) - beta = -lz2; + dAddScaledVector3r4(a, vrot, lz2); else if (beta > lz2) - beta = lz2; - dAddScaledVector3r4(a, vrot, -beta); - return c->radius - dCalcVectorLength3(a); + dAddScaledVector3r4(a, vrot, -lz2); + else + dAddScaledVector3r4(a, vrot, -beta); + return ((dxCapsule*)g)->radius - dCalcVectorLength3(a); } int dCollideCapsuleSphere (dxGeom *o1, dxGeom *o2, int flags, @@ -142,35 +138,33 @@ int dCollideCapsuleSphere (dxGeom *o1, dxGeom *o2, int flags, dIASSERT (o2->type == dSphereClass); dIASSERT ((flags & NUMC_MASK) >= 1); - dxCapsule *ccyl = (dxCapsule*) o1; - dxSphere *sphere = (dxSphere*) o2; - contact->g1 = o1; contact->g2 = o2; contact->side1 = -1; contact->side2 = -1; - dReal *pos = o1->final_posr->pos; - dReal *cR = o1->final_posr->R; + dxPosR *final_posr1 = o1->final_posr; + dReal *pos = final_posr1->pos; dReal *spherepos = o2->final_posr->pos; dVector3 diff; - dVector3 vrot; - dGetMatrixColumn3(vrot, cR, 2); dSubtractVectors3r4(diff, spherepos, pos); - // find the point on the cylinder axis that is closest to the sphere - dReal alpha = dCalcVectorDot3(diff, vrot); - dReal lz2 = ccyl->halfLenZ; - if (alpha > lz2) - alpha = lz2; - if (alpha < -lz2) - alpha = -lz2; + dVector3 vrot; + dGetMatrixColumn3(vrot, final_posr1->R, 2); - // collide the spheres - dVector3 p; - dAddScaledVector3(p, pos, vrot, alpha); - return dCollideSpheres (p, ccyl->radius, spherepos, sphere->radius, contact); + // find the point on the cylinder axis that is closest to the sphere + dVector3 p; + dReal lz2 = ((dxCapsule*)o1)->halfLenZ; + dReal alpha = dCalcVectorDot3(diff, vrot); + if (alpha > lz2) + dAddScaledVector3(p, pos, vrot, lz2); + if (alpha < -lz2) + dAddScaledVector3(p, pos, vrot, -lz2); + else + dAddScaledVector3(p, pos, vrot, alpha); + + return dCollideSpheres (p, ((dxCapsule*)o1)->radius, spherepos, ((dxSphere*)o2)->radius, contact); } @@ -186,11 +180,10 @@ int dCollideCapsuleBox (dxGeom *o1, dxGeom *o2, int flags, // get p1,p2 = cylinder axis endpoints, get radius - dReal *pos = o1->final_posr->pos; - dReal *cR = o1->final_posr->R; - + dxPosR *final_posr1 = o1->GetRecomputePosR(); + dReal *pos = final_posr1->pos; + dReal *cR = final_posr1->R; dReal clen = ((dxCapsule*)o1)->halfLenZ; - dReal radius = ((dxCapsule*)o1)->radius; dGetMatrixColumn3(vaxis, cR, 2); dScaleVector3r4(vaxis, clen); @@ -199,8 +192,9 @@ int dCollideCapsuleBox (dxGeom *o1, dxGeom *o2, int flags, dSubtractVectors3r4(p2, pos, vaxis); // copy out box center, rotation matrix, and side array - dReal *c = o2->final_posr->pos; - dReal *R = o2->final_posr->R; + dxPosR *final_posr2 = o2->GetRecomputePosR(); + dReal *c = final_posr2->pos; + dReal *R = final_posr2->R; const dReal *side = ((dxBox*)o2)->halfside; // get the closest point between the cylinder axis and the box @@ -217,6 +211,7 @@ int dCollideCapsuleBox (dxGeom *o1, dxGeom *o2, int flags, // consider capsule as box dVector3 normal; dReal depth; + dReal radius = ((dxCapsule*)o1)->radius; const dVector3 capboxside = {radius, radius, clen + radius}; int num = dBoxBox (c, R, side, pos, cR, capboxside, @@ -236,6 +231,7 @@ int dCollideCapsuleBox (dxGeom *o1, dxGeom *o2, int flags, else { // generate contact point + dReal radius = ((dxCapsule*)o1)->radius; if (dCollideSpheres(pl, radius, pb, 0, contact)) { contact->g1 = o1; @@ -271,11 +267,13 @@ int dCollideCapsuleCapsule (dxGeom *o1, dxGeom *o2, dReal lz2 = ((dxCapsule*)o2)->halfLenZ; dReal radius2 = ((dxCapsule*)o2)->radius; - dReal *pos1 = o1->final_posr->pos; - dReal *pos2 = o2->final_posr->pos; + dxPosR *dpr1 = o1->GetRecomputePosR(); + dReal *pos1 = dpr1->pos; + dGetMatrixColumn3(axis1, dpr1->R, 2); - dGetMatrixColumn3(axis1, o1->final_posr->R, 2); - dGetMatrixColumn3(axis2, o2->final_posr->R, 2); + dxPosR *dpr2 = o2->GetRecomputePosR(); + dReal *pos2 = dpr2->pos; + dGetMatrixColumn3(axis2, dpr2->R, 2); // if the cylinder axes are close to parallel, we'll try to detect up to // two contact points along the body of the cylinder. if we can't find any @@ -298,6 +296,7 @@ int dCollideCapsuleCapsule (dxGeom *o1, dxGeom *o2, { dNegateVector3r4(axis2); } + dSubtractVectors3r4(tt, pos1, pos2); dReal k = dCalcVectorDot3(axis1, tt); dReal a1lo = -lz1; @@ -369,12 +368,13 @@ int dCollideCapsulePlane(dxGeom *o1, dxGeom *o2, int flags, dVector3& planeNorm = *(dVector3*)((dxPlane*)o2)->p; dReal planeOffset = ((dxPlane*)o2)->p[3]; - dReal *pos = o1->final_posr->pos; - dReal *cR = o1->final_posr->R; + dxPosR *dpr = o1->GetRecomputePosR(); + dReal *pos = dpr->pos; + + dGetMatrixColumn3(vrot, dpr->R, 2); dReal capRadius = ((dxCapsule*)o1)->radius; - dGetMatrixColumn3(vrot, cR, 2); // collide the deepest capping sphere with the plane dReal lzsign = (dCalcVectorDot3(planeNorm, vrot) > 0) ? -(((dxCapsule*)o1)->halfLenZ) : ((dxCapsule*)o1)->halfLenZ; @@ -389,7 +389,8 @@ int dCollideCapsulePlane(dxGeom *o1, dxGeom *o2, int flags, contact->depth = depth; int ncontacts = 1; - if ((flags & NUMC_MASK) >= 2) { + if ((flags & NUMC_MASK) >= 2) + { // collide the other capping sphere with the plane dAddScaledVector3r4(p, pos, vrot, -lzsign); k = dCalcVectorDot3 (p, planeNorm); diff --git a/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_kernel.cpp b/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_kernel.cpp index 6e766a04..0cf45459 100644 --- a/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_kernel.cpp +++ b/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_kernel.cpp @@ -466,11 +466,9 @@ void dxGeom::computePosr() dIASSERT(offset_posr); dIASSERT(body); - dMultiply0_331 (final_posr->pos,body->posr.R,offset_posr->pos); - final_posr->pos[0] += body->posr.pos[0]; - final_posr->pos[1] += body->posr.pos[1]; - final_posr->pos[2] += body->posr.pos[2]; - dMultiply0_333 (final_posr->R,body->posr.R,offset_posr->R); + dMultiply0_331 (final_posr->pos, body->posr.R, offset_posr->pos); + dAddVector3r4 (final_posr->pos, body->posr.pos); + dMultiply0_333 (final_posr->R, body->posr.R, offset_posr->R); } bool dxGeom::controlGeometry(int /*controlClass*/, int /*controlCode*/, void * /*dataValue*/, int *dataSize) @@ -663,7 +661,7 @@ void dGeomCopyPosition(dxGeom *g, dVector3 pos) } -const dReal * dGeomGetRotation (dxGeom *g) +const dReal* dGeomGetRotation (dxGeom *g) { dAASSERT (g); dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); @@ -672,6 +670,7 @@ const dReal * dGeomGetRotation (dxGeom *g) } + void dGeomCopyRotation(dxGeom *g, dMatrix3 R) { dAASSERT (g); @@ -817,7 +816,7 @@ void dGeomGetPosRelPoint (dGeomID g, dReal px, dReal py, dReal pz, dVector3 resu prel[0] = px; prel[1] = py; prel[2] = pz; - dSubtractVector3r4(prel, g->final_posr->pos); + dSubtractVectors3r4(prel, g->final_posr->pos); prel[3] = 0; dMultiply1_331 (result, g->final_posr->R, prel); } diff --git a/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_kernel.h b/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_kernel.h index 87b4acb4..940ad79f 100644 --- a/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_kernel.h +++ b/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_kernel.h @@ -130,12 +130,23 @@ struct dxGeom : public dBase { // recalculate our new final position if needed void recomputePosr() { - if (gflags & GEOM_POSR_BAD) { + if (gflags & GEOM_POSR_BAD) + { computePosr(); gflags &= ~GEOM_POSR_BAD; } } + ODE_INLINE dxPosR* GetRecomputePosR() + { + if (gflags & GEOM_POSR_BAD) + { + computePosr(); + gflags &= ~GEOM_POSR_BAD; + } + return final_posr; + }; + bool checkControlValueSizeValidity(void *dataValue, int *dataSize, int iRequiresSize) { return (*dataSize == iRequiresSize && dataValue != 0) ? true : !(*dataSize = iRequiresSize); } // Here it is the intent to return true for 0 required size in any case virtual bool controlGeometry(int controlClass, int controlCode, void *dataValue, int *dataSize); diff --git a/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_box.cpp b/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_box.cpp index d4ffcea3..1dbf2ed9 100644 --- a/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_box.cpp +++ b/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_box.cpp @@ -34,6 +34,7 @@ #include "matrix.h" #include "odemath.h" #include "collision_util.h" +#include "collision_std.h" #include "collision_trimesh_internal.h" static void GenerateContact(int in_Flags, dContactGeom* in_Contacts, int in_Stride, @@ -71,9 +72,6 @@ struct sTrimeshBoxColliderData dVector3 m_vBoxHalfSize; dMatrix3 m_BoxRotTransposed; - // mesh data - dVector3 m_vHullDstPos; - // global collider data dVector3 m_vBestNormal; dReal m_fBestDepth; @@ -982,24 +980,18 @@ ODE_INLINE bool sTrimeshBoxColliderData::_cldTestOneTriangle(const dVector3 &v0, return false; } -void sTrimeshBoxColliderData::SetupInitialContext(dxTriMesh *TriMesh, dxGeom *BoxGeom, +ODE_INLINE void sTrimeshBoxColliderData::SetupInitialContext(dxTriMesh *TriMesh, dxGeom *BoxGeom, int Flags, dContactGeom* Contacts, int Stride) { // get source hull position, orientation and half size - const dMatrix3& mRotBox = *(const dMatrix3*)dGeomGetRotation(BoxGeom); - const dVector3& vPosBox = *(const dVector3*)dGeomGetPosition(BoxGeom); + dxPosR* posr = BoxGeom->GetRecomputePosR(); + const dMatrix3& mRotBox = *(const dMatrix3*)posr->R; + const dVector3& vPosBox = *(const dVector3*)posr->pos; dTransposetMatrix34(m_BoxRotTransposed, mRotBox); dCopyVector3r4(m_vHullBoxPos, vPosBox); - dGeomBoxGetLengths(BoxGeom, m_vBoxHalfSize); - dScaleVector3r4(m_vBoxHalfSize, 0.5f); - - // get destination hull position and orientation - const dVector3& vPosMesh = *(const dVector3*)dGeomGetPosition(TriMesh); - - // to global - dCopyVector3r4(m_vHullDstPos, vPosMesh); + dCopyVector3r4(m_vBoxHalfSize, ((dxBox*)BoxGeom)->halfside); // global info for contact creation m_ctContacts = 0; @@ -1011,9 +1003,7 @@ void sTrimeshBoxColliderData::SetupInitialContext(dxTriMesh *TriMesh, dxGeom *Bo // reset stuff m_fBestDepth = MAXVALUE; - m_vBestNormal[0] = 0; - m_vBestNormal[1] = 0; - m_vBestNormal[2] = 0; + dZeroVector3r4(m_vBestNormal); } int sTrimeshBoxColliderData::TestCollisionForSingleTriangle(int ctContacts0, int Triint, @@ -1151,8 +1141,9 @@ int dCollideBTL(dxGeom* g1, dxGeom* BoxGeom, int Flags, dContactGeom* Contacts, } // get destination hull position and orientation - const dMatrix3& mRotMesh = *(const dMatrix3*)dGeomGetRotation(TriMesh); - const dVector3& vPosMesh = *(const dVector3*)dGeomGetPosition(TriMesh); + dxPosR* TriMeshPosr = TriMesh->GetRecomputePosR(); + const dMatrix3& mRotMesh = *(const dMatrix3*)TriMeshPosr->R; + const dVector3& vPosMesh = *(const dVector3*)TriMeshPosr->pos; int ctContacts0 = 0; diff --git a/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_ccylinder.cpp b/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_capsule.cpp similarity index 72% rename from trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_ccylinder.cpp rename to trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_capsule.cpp index 0988f2ac..5ed43182 100644 --- a/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_ccylinder.cpp +++ b/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_capsule.cpp @@ -97,8 +97,10 @@ struct sTrimeshCapsuleColliderData #endif int _ProcessLocalContacts(dContactGeom *contact, dxTriMesh *TriMesh, dxGeom *Capsule); - static BOOL _cldClipEdgeToPlane(dVector3 &vEpnt0, dVector3 &vEpnt1, const dVector4& plPlane); - BOOL _cldTestAxis(dVector3 vAxis, int iAxis, BOOL bNoFlip = FALSE); + static BOOL sTrimeshCapsuleColliderData::_cldClipEdgeToPlaneNormNoOffset(dVector3 &vEpnt0, dVector3 &vEpnt1, const dVector3 plPlaneNormal); + static BOOL sTrimeshCapsuleColliderData::_cldClipEdgeToPlaneNorm(dVector3 &vEpnt0, dVector3 &vEpnt1, const dVector3 plPlaneNormal, dReal PlaneOffset); + BOOL sTrimeshCapsuleColliderData::_cldTestNormal(const dVector3 vAxis, int iAxis); + BOOL _cldTestAxis(const dVector3 vAxis, int iAxis); BOOL _cldTestSeparatingAxesOfCapsule(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2, uint8 flags); void _cldTestOneTriangleVSCapsule(const dVector3 &v0, const dVector3 &v1, @@ -109,7 +111,7 @@ struct sTrimeshCapsuleColliderData // capsule data // real time data - dMatrix3 m_mCapsuleRotation; + dMatrix3 m_mCapsuleRotation; dVector3 m_vCapsulePosition; dVector3 m_vCapsuleAxis; dVector3 m_vSizeOnAxis; @@ -126,8 +128,7 @@ struct sTrimeshCapsuleColliderData // global collider data dVector3 m_vNormal; dReal m_fBestDepth; - dReal m_fBestCenter; - dReal m_fBestrt; + dReal m_fBestCenterrt; int m_iBestAxis; dVector3 m_vN; @@ -265,143 +266,175 @@ int sTrimeshCapsuleColliderData::_ProcessLocalContacts(dContactGeom *contact, return nFinalContact; } -BOOL sTrimeshCapsuleColliderData::_cldClipEdgeToPlane( - dVector3 &vEpnt0, dVector3 &vEpnt1, const dVector4& plPlane) + +BOOL sTrimeshCapsuleColliderData::_cldClipEdgeToPlaneNorm( + dVector3 &vEpnt0, dVector3 &vEpnt1, const dVector3 plPlane, const dReal offset) { // calculate distance of edge points to plane - - dReal fDistance0 = dCalcPointPlaneDistance(vEpnt0, plPlane); - dReal fDistance1 = dCalcPointPlaneDistance(vEpnt1, plPlane); - + dReal fDistance0 = dCalcVectorDot3(vEpnt0, plPlane) + offset; + dReal fDistance1 = dCalcVectorDot3(vEpnt1, plPlane) + offset; // if both points are behind the plane - if ( fDistance0 < 0 && fDistance1 < 0 ) - { - // do nothing + if (fDistance0 < dEpsilon && fDistance1 < dEpsilon) return FALSE; - // if both points in front of the plane - } else if ( fDistance0 >= 0 && fDistance1 >= 0 ) - { - // accept them + + // if both points in front of the plane + if (fDistance0 >= 0 && fDistance1 >= 0) return TRUE; - // if we have edge/plane intersection - } else + + // find intersection point of edge and plane + dReal factor = fDistance0 / (fDistance0 - fDistance1); + // clamp correct edge to intersection point + if (fDistance0 < 0) { - // find intersection point of edge and plane - dVector3 vIntersectionPoint; - dReal diffFactor = fDistance0/(fDistance0-fDistance1); - dSubtractVectors3r4(vIntersectionPoint, vEpnt0, vEpnt1); - dScaleVector3r4(vIntersectionPoint, diffFactor); - dSubtractVectors3r4(vIntersectionPoint, vEpnt0, vIntersectionPoint); - // clamp correct edge to intersection point - if ( fDistance0 < 0 ) - { - dCopyVector3r4(vEpnt0,vIntersectionPoint); - } else - { - dCopyVector3r4(vEpnt1,vIntersectionPoint); - } - return TRUE; + dCalcLerpVectors3r4(vEpnt0, vEpnt0, vEpnt1, factor); + } + else + { + dCalcLerpVectors3r4(vEpnt1, vEpnt0, vEpnt1, factor); } return TRUE; } -BOOL sTrimeshCapsuleColliderData::_cldTestAxis( - dVector3 vAxis, - int iAxis, - BOOL bNoFlip/* = FALSE*/) +BOOL sTrimeshCapsuleColliderData::_cldClipEdgeToPlaneNormNoOffset( + dVector3 &vEpnt0, dVector3 &vEpnt1, const dVector3 plPlane) { + // calculate distance of edge points to plane + dReal fDistance0 = dCalcVectorDot3(vEpnt0, plPlane); + dReal fDistance1 = dCalcVectorDot3(vEpnt1, plPlane); - // calculate length of separating axis vector - dReal fL = dCalcVectorLengthSquare3(vAxis); - // if not long enough - // TODO : dReal epsilon please - if ( fL < REAL(1e-6) ) - { - // do nothing - //iLastOutAxis = 0; + // if both points are behind the plane + if (fDistance0 < dEpsilon && fDistance1 < dEpsilon) + return FALSE; + + // if both points in front of the plane + if (fDistance0 >= 0 && fDistance1 >= 0) return TRUE; + + // find intersection point of edge and plane + dReal factor = fDistance0 / (fDistance0 - fDistance1); + // clamp correct edge to intersection point + if (fDistance0 < 0) + { + dCalcLerpVectors3r4(vEpnt0, vEpnt0, vEpnt1, factor); + } + else + { + dCalcLerpVectors3r4(vEpnt1, vEpnt0, vEpnt1, factor); + } + return TRUE; +} + +BOOL sTrimeshCapsuleColliderData::_cldTestAxis(const dVector3 vAxis, int iAxis) +{ + dReal min = dCalcVectorDot3(m_vV0, vAxis); + dReal max = dCalcVectorDot3(m_vV1, vAxis); + dReal tmp = dCalcVectorDot3(m_vV2, vAxis); + if (min > max) + { + dReal tmp2 = max < tmp ? max : tmp; + max = min > tmp ? min : tmp; + min = tmp2; + } + else + { + if (tmp < min) + min = tmp; + if (tmp > max) + max = tmp; } - // otherwise normalize it -// dNormalize3(vAxis); - fL = dSqrt(fL); - fL = REAL(1.0)/fL; - - dScaleVector3r4(vAxis, fL); + // find triangle's center of interval on axis + dReal fCenter = (min + max) * REAL(0.5); + // calculate triangles half interval + dReal fTriangleRadius = max - fCenter; // project capsule on vAxis dReal frc = dFabs(dCalcVectorDot3(m_vSizeOnAxis, vAxis)) + m_fCapsuleRadius; - // project triangle on vAxis - dReal afv[3]; - afv[0] = dCalcVectorDot3(m_vV0, vAxis); - afv[1] = dCalcVectorDot3(m_vV1, vAxis); - afv[2] = dCalcVectorDot3(m_vV2, vAxis); - - dReal fMin = MAX_REAL; - dReal fMax = MIN_REAL; - - // for each vertex - for(int i = 0; i < 3; i++) - { - // find minimum - if (afv[i] < fMin) - { - fMin = afv[i]; - } - // find maximum - if (afv[i] > fMax) - { - fMax = afv[i]; - } - } - - // find triangle's center of interval on axis - dReal fCenter = (fMin + fMax) * REAL(0.5); - // calculate triangles half interval - dReal fTriangleRadius = (fMax - fMin) * REAL(0.5); - - dReal frcPlusTRadius = frc + fTriangleRadius; - // if they do not overlap, - if (dFabs(fCenter) > frcPlusTRadius) - { - // exit, we have no intersection - return FALSE; - } - // calculate depth + dReal frcPlusTRadius = frc + fTriangleRadius; dReal fDepth = dFabs(fCenter) - frcPlusTRadius; + // if they do not overlap, + if (fDepth > 0) + { + // exit, we have no intersection + return FALSE; + } + // if greater then best found so far - if ( fDepth > m_fBestDepth ) + if (fDepth * dReal(1.5) > m_fBestDepth) { // remember depth - m_fBestDepth = fDepth; - m_fBestrt = fTriangleRadius; - m_iBestAxis = iAxis; + m_fBestDepth = fDepth; + m_iBestAxis = iAxis; // flip normal if interval is wrong faced - if (fCenter < 0 && !bNoFlip) - { + if (fCenter < 0) + { dCopyNegatedVector3r4(m_vNormal, vAxis); - m_fBestCenter = -fCenter; + m_fBestCenterrt = -fCenter - fTriangleRadius; } else { dCopyVector3r4(m_vNormal, vAxis); - m_fBestCenter = fCenter; + m_fBestCenterrt = fCenter - fTriangleRadius; } } return TRUE; } +BOOL sTrimeshCapsuleColliderData::_cldTestNormal(const dVector3 vAxis, int iAxis) +{ + dReal min = dCalcVectorDot3(m_vV0, vAxis); + dReal max = dCalcVectorDot3(m_vV1, vAxis); + dReal tmp = dCalcVectorDot3(m_vV2, vAxis); + if (min > max) + { + dReal tmp2 = max < tmp ? max : tmp; + max = min > tmp ? min : tmp; + min = tmp2; + } + else + { + if (tmp < min) + min = tmp; + if (tmp > max) + max = tmp; + } + + // find triangle's center of interval on axis + dReal fCenter = (min + max) * REAL(0.5); + // calculate triangles half interval + dReal fTriangleRadius = max - fCenter; + + // project capsule on vAxis + dReal frc = dFabs(dCalcVectorDot3(m_vSizeOnAxis, vAxis)) + m_fCapsuleRadius; + + // calculate depth + dReal frcPlusTRadius = frc + fTriangleRadius; + dReal fDepth = dFabs(fCenter) - frcPlusTRadius; + + // if they do not overlap, + if (fDepth > 0) + { + // exit, we have no intersection + return FALSE; + } + + //always first remember depth + m_fBestDepth = fDepth; + m_iBestAxis = iAxis; + + dCopyVector3r4(m_vNormal, vAxis); + m_fBestCenterrt = fCenter - fTriangleRadius; + + return TRUE; +} + // helper for less key strokes -inline void _CalculateAxis(const dVector3& v1, - const dVector3& v2, - const dVector3& v3, - const dVector3& v4, - dVector3& r) +inline void _CalculateAxis(const dVector3& v1, const dVector3& v2, const dVector3& v3, const dVector3& v4, dVector3& r) { dVector3 t1; dVector3 t2; @@ -412,119 +445,126 @@ inline void _CalculateAxis(const dVector3& v1, } BOOL sTrimeshCapsuleColliderData::_cldTestSeparatingAxesOfCapsule( - const dVector3 &v0, - const dVector3 &v1, - const dVector3 &v2, - uint8 flags) + const dVector3 &v0, const dVector3 &v1, const dVector3 &v2, uint8 flags) { - // calculate caps centers in absolute space - dVector3 vCp0; - dAddVectors3r4(vCp0, m_vCapsulePosition, m_vSizeOnAxis); - dVector3 vCp1; - dSubtractVectors3r4(vCp1, m_vCapsulePosition, m_vSizeOnAxis); - - // reset best axis - m_iBestAxis = 0; - // reset best depth - m_fBestDepth = -MAX_REAL; - // reset separating axis vector - dVector3 vAxis = {REAL(0.0), REAL(0.0), REAL(0.0), REAL(0.0)}; - - // Epsilon value for checking axis vector length - const dReal fEpsilon = 1e-6f; - - // We begin to test for 19 separating axis now - // I wonder does it help if we employ the method like ISA-GJK??? - // Or at least we should do experiment and find what axis will - // be most likely to be separating axis to check it first. - // Translate triangle to Cc cord. // used in _cldTestAxis dSubtractVectors3r4(m_vV0, v0, m_vCapsulePosition); dSubtractVectors3r4(m_vV1, v1, m_vCapsulePosition); dSubtractVectors3r4(m_vV2, v2, m_vCapsulePosition); + // reset best axis + m_iBestAxis = 0; + // reset best depth + m_fBestDepth = MIN_REAL; + // reset separating axis vector + dVector3 vAxis; + + // We begin to test for 19 separating axis now + // I wonder does it help if we employ the method like ISA-GJK??? + // Or at least we should do experiment and find what axis will + // be most likely to be separating axis to check it first. + + // Original // axis m_vN //vAxis = -m_vN; dCopyNegatedVector3r4(vAxis, m_vN); - if (!_cldTestAxis(vAxis, 1, TRUE)) + if (!_cldTestNormal(vAxis, 1)) { return FALSE; } + if (flags == 0) + return TRUE; + + dVector3 vCp0; + dVector3 vCp1; + + dAddVectors3r4(vCp0, m_vCapsulePosition, m_vSizeOnAxis); + dSubtractVectors3r4(vCp1, m_vCapsulePosition, m_vSizeOnAxis); + if (flags & dxTriMeshData::kEdge0) { // axis CxE0 - Edge 0 dCalcVectorCross3r4(vAxis, m_vCapsuleAxis, m_vE0); - if (!_cldTestAxis(vAxis, 2)) - return FALSE; + if (dSafeNormalize3fast(vAxis)) + { + if (!_cldTestAxis(vAxis, 2)) + return FALSE; + } + + // second capsule point + // axis ((Cp1-V0) x E0) x E0 + _CalculateAxis(vCp1, v0, m_vE0, m_vE0, vAxis); + if (dSafeNormalize3fast(vAxis)) + { + if (!_cldTestAxis(vAxis, 8)) + return FALSE; + } + + // first capsule point + // axis ((Cp0-V0) x E0) x E0 + _CalculateAxis(vCp0, v0, m_vE0, m_vE0, vAxis); + if (dSafeNormalize3fast(vAxis)) + { + if (!_cldTestAxis(vAxis, 5)) + return FALSE; + } } if (flags & dxTriMeshData::kEdge1) { // axis CxE1 - Edge 1 dCalcVectorCross3r4(vAxis, m_vCapsuleAxis, m_vE1); - if (!_cldTestAxis(vAxis, 3)) - return FALSE; + if (dSafeNormalize3fast(vAxis)) + { + if (!_cldTestAxis(vAxis, 3)) + return FALSE; + } + + // axis ((Cp0-V1) x E1) x E1 + _CalculateAxis(vCp0, v1, m_vE1, m_vE1, vAxis); + if (dSafeNormalize3fast(vAxis)) + { + if (!_cldTestAxis(vAxis, 6)) + return FALSE; + } + + // axis ((Cp1-V1) x E1) x E1 + _CalculateAxis(vCp1, v1, m_vE1, m_vE1, vAxis); + if (dSafeNormalize3fast(vAxis)) + { + if (!_cldTestAxis(vAxis, 9)) + return FALSE; + } } if (flags & dxTriMeshData::kEdge2) { // axis CxE2 - Edge 2 dCalcVectorCross3r4(vAxis, m_vCapsuleAxis, m_vE2); - if (!_cldTestAxis(vAxis, 4)) - return FALSE; - } + if (dSafeNormalize3fast(vAxis)) + { + if (!_cldTestAxis(vAxis, 4)) + return FALSE; + } - if (flags & dxTriMeshData::kEdge0) - { - // first capsule point - // axis ((Cp0-V0) x E0) x E0 - _CalculateAxis(vCp0, v0, m_vE0, m_vE0, vAxis); - if (!_cldTestAxis(vAxis, 5)) - return FALSE; - } - - if (flags & dxTriMeshData::kEdge1) - { - // axis ((Cp0-V1) x E1) x E1 - _CalculateAxis(vCp0, v1, m_vE1, m_vE1, vAxis); - if (!_cldTestAxis(vAxis, 6)) - return FALSE; - } - - if (flags & dxTriMeshData::kEdge2) - { // axis ((Cp0-V2) x E2) x E2 _CalculateAxis(vCp0, v2, m_vE2, m_vE2, vAxis); - if (!_cldTestAxis(vAxis, 7)) + if (dSafeNormalize3fast(vAxis)) + { + if (!_cldTestAxis(vAxis, 7)) return FALSE; - } + } - if (flags & dxTriMeshData::kEdge0) - { - // second capsule point - // axis ((Cp1-V0) x E0) x E0 - _CalculateAxis(vCp1, v0, m_vE0, m_vE0, vAxis); - if (!_cldTestAxis(vAxis, 8)) - return FALSE; - } - - if (flags & dxTriMeshData::kEdge1) - { - // axis ((Cp1-V1) x E1) x E1 - _CalculateAxis(vCp1, v1, m_vE1, m_vE1, vAxis); - if (!_cldTestAxis(vAxis, 9)) - return FALSE; - } - - if (flags & dxTriMeshData::kEdge2) - { // axis ((Cp1-V2) x E2) x E2 _CalculateAxis(vCp1, v2, m_vE2, m_vE2, vAxis); - if (!_cldTestAxis(vAxis, 10)) - return FALSE; + if (dSafeNormalize3fast(vAxis)) + { + if (!_cldTestAxis(vAxis, 10)) + return FALSE; + } } if (flags & dxTriMeshData::kVert0) @@ -532,8 +572,29 @@ BOOL sTrimeshCapsuleColliderData::_cldTestSeparatingAxesOfCapsule( // first vertex on triangle // axis ((V0-Cp0) x C) x C _CalculateAxis(v0, vCp0, m_vCapsuleAxis, m_vCapsuleAxis, vAxis); - if (!_cldTestAxis(vAxis, 11)) - return FALSE; + if (dSafeNormalize3fast(vAxis)) + { + if (!_cldTestAxis(vAxis, 11)) + return FALSE; + } + + // first triangle vertex and first capsule point + //vAxis = v0 - vCp0; + dSubtractVectors3r4(vAxis, v0, vCp0); + if (dSafeNormalize3fast(vAxis)) + { + if (!_cldTestAxis(vAxis, 14)) + return FALSE; + } + + // first triangle vertex and second capsule point + //vAxis = v0 - vCp1; + dSubtractVectors3r4(vAxis, v0, vCp1); + if (dSafeNormalize3fast(vAxis)) + { + if (!_cldTestAxis(vAxis, 17)) + return FALSE; + } } if (flags & dxTriMeshData::kVert1) @@ -541,8 +602,29 @@ BOOL sTrimeshCapsuleColliderData::_cldTestSeparatingAxesOfCapsule( // second vertex on triangle // axis ((V1-Cp0) x C) x C _CalculateAxis(v1, vCp0, m_vCapsuleAxis, m_vCapsuleAxis, vAxis); - if (!_cldTestAxis(vAxis, 12)) - return FALSE; + if (dSafeNormalize3fast(vAxis)) + { + if (!_cldTestAxis(vAxis, 12)) + return FALSE; + } + + // second triangle vertex and first capsule point + //vAxis = v1 - vCp0; + dSubtractVectors3r4(vAxis, v1, vCp0); + if (dSafeNormalize3fast(vAxis)) + { + if (!_cldTestAxis(vAxis, 15)) + return FALSE; + } + + // second triangle vertex and second capsule point + //vAxis = v1 - vCp1; + dSubtractVectors3r4(vAxis, v1, vCp1); + if (dSafeNormalize3fast(vAxis)) + { + if (!_cldTestAxis(vAxis, 18)) + return FALSE; + } } if (flags & dxTriMeshData::kVert2) @@ -550,65 +632,29 @@ BOOL sTrimeshCapsuleColliderData::_cldTestSeparatingAxesOfCapsule( // third vertex on triangle // axis ((V2-Cp0) x C) x C _CalculateAxis(v2, vCp0, m_vCapsuleAxis, m_vCapsuleAxis, vAxis); - if (!_cldTestAxis(vAxis, 13)) - return FALSE; - } + if (dSafeNormalize3fast(vAxis)) + { + if (!_cldTestAxis(vAxis, 13)) + return FALSE; + } - // Test as separating axes direction vectors between each triangle - // edge and each capsule's cap center - - if (flags & dxTriMeshData::kVert0) - { - // first triangle vertex and first capsule point - //vAxis = v0 - vCp0; - dSubtractVectors3r4(vAxis, v0, vCp0); - if (!_cldTestAxis(vAxis, 14)) - return FALSE; - } - - if (flags & dxTriMeshData::kVert1) - { - // second triangle vertex and first capsule point - //vAxis = v1 - vCp0; - dSubtractVectors3r4(vAxis, v1, vCp0); - if (!_cldTestAxis(vAxis, 15)) - return FALSE; - } - - if (flags & dxTriMeshData::kVert2) - { // third triangle vertex and first capsule point //vAxis = v2 - vCp0; dSubtractVectors3r4(vAxis, v2, vCp0); - if (!_cldTestAxis(vAxis, 16)) - return FALSE; - } + if (dSafeNormalize3fast(vAxis)) + { + if (!_cldTestAxis(vAxis, 16)) + return FALSE; + } - if (flags & dxTriMeshData::kVert0) - { - // first triangle vertex and second capsule point - //vAxis = v0 - vCp1; - dSubtractVectors3r4(vAxis, v0, vCp1); - if (!_cldTestAxis(vAxis, 17)) - return FALSE; - } - - if (flags & dxTriMeshData::kVert1) - { - // second triangle vertex and second capsule point - //vAxis = v1 - vCp1; - dSubtractVectors3r4(vAxis, v1, vCp1); - if (!_cldTestAxis(vAxis, 18)) - return FALSE; - } - - if (flags & dxTriMeshData::kVert2) - { // third triangle vertex and second capsule point //vAxis = v2 - vCp1; dSubtractVectors3r4(vAxis, v2, vCp1); - if (!_cldTestAxis(vAxis, 19)) - return FALSE; + if (dSafeNormalize3fast(vAxis)) + { + if (!_cldTestAxis(vAxis, 19)) + return FALSE; + } } return TRUE; } @@ -624,7 +670,7 @@ void sTrimeshCapsuleColliderData::_cldTestOneTriangleVSCapsule( dSubtractVectors3r4(m_vE2, v0, v2); // calculate poly normal (negative) - dCalcVectorCross3r4(m_vN,m_vE0, m_vE1); + dCalcVectorCross3r4(m_vN, m_vE0, m_vE1); // Even though all triangles might be initially valid, // a triangle may degenerate into a segment after applying @@ -634,13 +680,8 @@ void sTrimeshCapsuleColliderData::_cldTestOneTriangleVSCapsule( return; } - // create plane from triangle - dReal plDistance = -dCalcVectorDot3(v0,m_vN); - dVector4 plTrianglePlane; - dConstructPlane(plTrianglePlane,m_vN,plDistance); - // calculate capsule distance to plane - dReal fDistanceCapsuleCenterToPlane = dCalcPointPlaneDistance(m_vCapsulePosition, plTrianglePlane); + dReal fDistanceCapsuleCenterToPlane = dCalcVectorDot3(m_vCapsulePosition, m_vN) - dCalcVectorDot3(v0, m_vN); // Capsule must be over positive side of triangle if (fDistanceCapsuleCenterToPlane < 0 && singleSide) @@ -661,9 +702,9 @@ void sTrimeshCapsuleColliderData::_cldTestOneTriangleVSCapsule( if (fDistanceCapsuleCenterToPlane < -m_fCapsuleSize) return; - dCopyVector3r4(vPnt0,v0); - dCopyVector3r4(vPnt1,v2); - dCopyVector3r4(vPnt2,v1); + dCopyVector3r4(vPnt0, v0); + dCopyVector3r4(vPnt1, v2); + dCopyVector3r4(vPnt2, v1); dCopyNegatedVector3r4(m_vN, m_vN); @@ -703,20 +744,16 @@ void sTrimeshCapsuleColliderData::_cldTestOneTriangleVSCapsule( // transform capsule edge points into triangle space dVector3 vCEdgePoint0; dAddVectors3r4(vCEdgePoint0, vCposTrans, m_vSizeOnAxis); - dSubtractVectors3(vCEdgePoint0, vCEdgePoint0, vPnt0); + dSubtractVectors3r4(vCEdgePoint0, vPnt0); dVector3 vCEdgePoint1; dSubtractVectors3(vCEdgePoint1, vCposTrans, m_vSizeOnAxis); - dSubtractVectors3(vCEdgePoint1, vCEdgePoint1, vPnt0); + dSubtractVectors3r4(vCEdgePoint1, vPnt0); - dVector4 plPlane; dVector3 _minus_vN; - dCopyNegatedVector3r4(_minus_vN, m_vN); - // triangle plane - dConstructPlane(plPlane,_minus_vN,0); - if (!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) + if (!_cldClipEdgeToPlaneNormNoOffset( vCEdgePoint0, vCEdgePoint1, _minus_vN)) { return; } @@ -724,34 +761,31 @@ void sTrimeshCapsuleColliderData::_cldTestOneTriangleVSCapsule( // plane edge 0 dVector3 vTemp; dCalcVectorCross3r4(vTemp, m_vN, m_vE0); - dConstructPlane(plPlane, vTemp, REAL(1e-5)); - if (!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) + if (!_cldClipEdgeToPlaneNormNoOffset( vCEdgePoint0, vCEdgePoint1, vTemp)) { return; } // plane with edge 1 dCalcVectorCross3r4(vTemp, m_vN, m_vE1); - dConstructPlane(plPlane, vTemp, -(dCalcVectorDot3(m_vE0, vTemp)- REAL(1e-5))); - if (!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) + if (!_cldClipEdgeToPlaneNorm( vCEdgePoint0, vCEdgePoint1, vTemp, -(dCalcVectorDot3(m_vE0, vTemp)))) { return; } // plane with edge 2 dCalcVectorCross3r4(vTemp,m_vN,m_vE2); - dConstructPlane(plPlane, vTemp, REAL(1e-5)); - if (!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) { + if (!_cldClipEdgeToPlaneNormNoOffset( vCEdgePoint0, vCEdgePoint1, vTemp)) + { return; } - // return capsule edge points into absolute space - dAddVector3r4(vCEdgePoint0, vPnt0); - dAddVector3r4(vCEdgePoint1, vPnt0); - // calculate depths for both contact points + dAddVector3r4(vCEdgePoint0, vPnt0); dSubtractVectors3r4(vTemp, vCEdgePoint0, m_vCapsulePosition); - dReal fDepth0 = dCalcVectorDot3(vTemp, m_vNormal) - (m_fBestCenter - m_fBestrt); + dReal fDepth0 = dCalcVectorDot3(vTemp, m_vNormal) - m_fBestCenterrt; + + dAddVector3r4(vCEdgePoint1, vPnt0); dSubtractVectors3r4(vTemp, vCEdgePoint1, m_vCapsulePosition); - dReal fDepth1 = dCalcVectorDot3(vTemp, m_vNormal) - (m_fBestCenter - m_fBestrt); + dReal fDepth1 = dCalcVectorDot3(vTemp, m_vNormal) - m_fBestCenterrt; // clamp depths to zero if (fDepth0 < 0) @@ -763,14 +797,14 @@ void sTrimeshCapsuleColliderData::_cldTestOneTriangleVSCapsule( // Cached contacts's data // contact 0 - dIASSERT(m_ctContacts < (m_iFlags & NUMC_MASK)); // Do not call function if there is no room to store result m_gLocalContacts[m_ctContacts].fDepth = fDepth0; dCopyVector3r4(m_gLocalContacts[m_ctContacts].vNormal, m_vNormal); dCopyVector3r4(m_gLocalContacts[m_ctContacts].vPos, vCEdgePoint0); m_gLocalContacts[m_ctContacts].nFlags = 1; m_ctContacts++; - if (m_ctContacts < (m_iFlags & NUMC_MASK)) { + if (m_ctContacts < (m_iFlags & NUMC_MASK)) + { // contact 1 m_gLocalContacts[m_ctContacts].fDepth = fDepth1; dCopyVector3r4(m_gLocalContacts[m_ctContacts].vNormal, m_vNormal); @@ -788,7 +822,7 @@ void sTrimeshCapsuleColliderData::SetupInitialContext(dxTriMesh *TriMesh, dxGeom const dVector3* pDst = (const dVector3*)dGeomGetPosition(Capsule); memcpy(m_vCapsulePosition, pDst, sizeof(dVector3)); - + m_vCapsuleAxis[0] = m_mCapsuleRotation[nCAPSULE_AXIS]; m_vCapsuleAxis[1] = m_mCapsuleRotation[4 + nCAPSULE_AXIS]; m_vCapsuleAxis[2] = m_mCapsuleRotation[8 + nCAPSULE_AXIS]; @@ -812,16 +846,6 @@ void sTrimeshCapsuleColliderData::SetupInitialContext(dxTriMesh *TriMesh, dxGeom // reset contact counter m_ctContacts = 0; - - // reset best depth - m_fBestDepth = - MAX_REAL; - m_fBestCenter = 0; - m_fBestrt = 0; - - // reset collision normal - m_vNormal[0] = REAL(0.0); - m_vNormal[1] = REAL(0.0); - m_vNormal[2] = REAL(0.0); } int sTrimeshCapsuleColliderData::TestCollisionForSingleTriangle(int ctContacts0, @@ -874,9 +898,6 @@ static void dQueryCCTLPotentialCollisionTriangles(OBBCollider &Collider, OBB obbCapsule(cCenter,cExtents,obbRot); - Matrix4x4 CapsuleMatrix; - MakeMatrix(vCapsulePosition, mCapsuleRotation, CapsuleMatrix); - Matrix4x4 MeshMatrix; MakeMatrix(cData.m_mTriMeshPos, cData.m_mTriMeshRot, MeshMatrix); @@ -917,7 +938,6 @@ int dCollideCCTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int s dIASSERT ((flags & NUMC_MASK) >= 1); int nContactCount = 0; - bool singleSide = true; dxTriMesh *TriMesh = (dxTriMesh*)o1; dxGeom *Capsule = o2; @@ -925,22 +945,6 @@ int dCollideCCTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int s sTrimeshCapsuleColliderData cData; cData.SetupInitialContext(TriMesh, Capsule, flags, skip); - uint8 meshflags = TriMesh->Data->meshFlags; - - int cntr = 0; - if ((meshflags & dxTriMeshData::closedSurface) == 0) - { - dReal size = REAL(1.5) * cData.m_fCapsuleRadius; - if(size < o1->aabb[1] - o1->aabb[0]) - cntr++; - if(size < o1->aabb[3] - o1->aabb[2]) - cntr++; - if(size < o1->aabb[5] - o1->aabb[4]) - cntr++; - if(cntr >= 1) - singleSide = false; - } - const unsigned uiTLSKind = TriMesh->getParentSpaceTLSKind(); dIASSERT(uiTLSKind == Capsule->getParentSpaceTLSKind()); // The colliding spaces must use matching cleanup method TrimeshCollidersCache *pccColliderCache = GetTrimeshCollidersCache(uiTLSKind); @@ -954,16 +958,25 @@ int dCollideCCTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int s { // Retrieve data int TriCount = Collider.GetNbTouchedPrimitives(); - if (TriCount != 0) { - const int* Triangles = (const int*)Collider.GetTouchedPrimitives(); + bool singleSide = true; + uint8 meshflags = TriMesh->Data->meshFlags; - if (TriMesh->ArrayCallback != null) + if ((meshflags & dxTriMeshData::closedSurface) == 0) { - TriMesh->ArrayCallback(TriMesh, Capsule, Triangles, TriCount); + dReal size = REAL(1.5) * cData.m_fCapsuleRadius; + dVector3& ext = TriMesh->Data->AABBExtents; + if (size < ext[0]) + singleSide = false; + else if (size < ext[1]) + singleSide = false; + else if (size < ext[2]) + singleSide = false; } + const int* Triangles = (const int*)Collider.GetTouchedPrimitives(); + // allocate buffer for local contacts on stack cData.m_gLocalContacts = (sLocalContactData*)dALLOCA16(sizeof(sLocalContactData)*(cData.m_iFlags & NUMC_MASK)); @@ -972,22 +985,39 @@ int dCollideCCTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int s uint8* UseFlags = TriMesh->Data->UseFlags; // loop through all intersecting triangles - for (int i = 0; i < TriCount; i++) + if (UseFlags) { - const int Triint = Triangles[i]; - if (!Callback(TriMesh, Capsule, Triint)) continue; - - dVector3 dv[3]; - FetchTriangle(TriMesh, Triint, cData.m_mTriMeshPos, cData.m_mTriMeshRot, dv); - - uint8 flags = UseFlags ? UseFlags[Triint] : (uint8)dxTriMeshData::kUseAll; - - bool bFinishSearching; - ctContacts0 = cData.TestCollisionForSingleTriangle(ctContacts0, Triint, dv, flags, bFinishSearching, singleSide); - - if (bFinishSearching) + for (int i = 0; i < TriCount; i++) { - break; + const int Triint = Triangles[i]; + + dVector3 dv[3]; + FetchTriangle(TriMesh, Triint, cData.m_mTriMeshPos, cData.m_mTriMeshRot, dv); + + bool bFinishSearching; + ctContacts0 = cData.TestCollisionForSingleTriangle(ctContacts0, Triint, dv, UseFlags[Triint], bFinishSearching, singleSide); + + if (bFinishSearching) + { + break; + } + } + } + else + { + for (int i = 0; i < TriCount; i++) + { + const int Triint = Triangles[i]; + dVector3 dv[3]; + FetchTriangle(TriMesh, Triint, cData.m_mTriMeshPos, cData.m_mTriMeshRot, dv); + + bool bFinishSearching; + ctContacts0 = cData.TestCollisionForSingleTriangle(ctContacts0, Triint, dv, (uint8)dxTriMeshData::kUseAll, bFinishSearching, singleSide); + + if (bFinishSearching) + { + break; + } } } diff --git a/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_distance.cpp b/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_distance.cpp deleted file mode 100644 index 2264722b..00000000 --- a/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_distance.cpp +++ /dev/null @@ -1,1277 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -// This file contains some code based on the code from Magic Software. -// That code is available under a Free Source License Agreement -// that can be found at http://www.magic-software.com/License/free.pdf - -#include -#include -#include "config.h" -#include "odemath.h" -#include "collision_trimesh_internal.h" - -//------------------------------------------------------------------------------ -/** - @brief Finds the shortest distance squared between a point and a triangle. - - @param pfSParam Barycentric coordinate of triangle at point closest to p (u) - @param pfTParam Barycentric coordinate of triangle at point closest to p (v) - @return Shortest distance squared. - - The third Barycentric coordinate is implicit, ie. w = 1.0 - u - v - - Taken from: - Magic Software, Inc. - http://www.magic-software.com -*/ -dReal SqrDistancePointTri( const dVector3 p, const dVector3 triOrigin, - const dVector3 triEdge0, const dVector3 triEdge1, - dReal* pfSParam, dReal* pfTParam ) -{ - dVector3 kDiff; - dSubtractVectors3r4(kDiff, triOrigin, p); - dReal fA00 = dCalcVectorLengthSquare3( triEdge0); - dReal fA01 = dCalcVectorDot3( triEdge0, triEdge1 ); - dReal fA11 = dCalcVectorLengthSquare3( triEdge1); - dReal fB0 = dCalcVectorDot3( kDiff, triEdge0 ); - dReal fB1 = dCalcVectorDot3( kDiff, triEdge1 ); - dReal fC = dCalcVectorLengthSquare3(kDiff); - dReal fDet = dReal(fabs(fA00*fA11-fA01*fA01)); - dReal fS = fA01*fB1-fA11*fB0; - dReal fT = fA01*fB0-fA00*fB1; - dReal fSqrDist; - - if ( fS + fT <= fDet ) - { - if ( fS < REAL(0.0) ) - { - if ( fT < REAL(0.0) ) // region 4 - { - if ( fB0 < REAL(0.0) ) - { - fT = REAL(0.0); - if ( -fB0 >= fA00 ) - { - fS = REAL(1.0); - fSqrDist = fA00+REAL(2.0)*fB0+fC; - } - else - { - fS = -fB0/fA00; - fSqrDist = fB0*fS+fC; - } - } - else - { - fS = REAL(0.0); - if ( fB1 >= REAL(0.0) ) - { - fT = REAL(0.0); - fSqrDist = fC; - } - else if ( -fB1 >= fA11 ) - { - fT = REAL(1.0); - fSqrDist = fA11+REAL(2.0)*fB1+fC; - } - else - { - fT = -fB1/fA11; - fSqrDist = fB1*fT+fC; - } - } - } - else // region 3 - { - fS = REAL(0.0); - if ( fB1 >= REAL(0.0) ) - { - fT = REAL(0.0); - fSqrDist = fC; - } - else if ( -fB1 >= fA11 ) - { - fT = REAL(1.0); - fSqrDist = fA11+REAL(2.0)*fB1+fC; - } - else - { - fT = -fB1/fA11; - fSqrDist = fB1*fT+fC; - } - } - } - else if ( fT < REAL(0.0) ) // region 5 - { - fT = REAL(0.0); - if ( fB0 >= REAL(0.0) ) - { - fS = REAL(0.0); - fSqrDist = fC; - } - else if ( -fB0 >= fA00 ) - { - fS = REAL(1.0); - fSqrDist = fA00+REAL(2.0)*fB0+fC; - } - else - { - fS = -fB0/fA00; - fSqrDist = fB0*fS+fC; - } - } - else // region 0 - { - // minimum at interior point - if ( fDet == REAL(0.0) ) - { - fS = REAL(0.0); - fT = REAL(0.0); - fSqrDist = dInfinity; - } - else - { - dReal fInvDet = REAL(1.0)/fDet; - fS *= fInvDet; - fT *= fInvDet; - fSqrDist = fS*(fA00*fS+fA01*fT+REAL(2.0)*fB0) + - fT*(fA01*fS+fA11*fT+REAL(2.0)*fB1)+fC; - } - } - } - else - { - dReal fTmp0, fTmp1, fNumer, fDenom; - - if ( fS < REAL(0.0) ) // region 2 - { - fTmp0 = fA01 + fB0; - fTmp1 = fA11 + fB1; - if ( fTmp1 > fTmp0 ) - { - fNumer = fTmp1 - fTmp0; - fDenom = fA00-REAL(2.0)*fA01+fA11; - if ( fNumer >= fDenom ) - { - fS = REAL(1.0); - fT = REAL(0.0); - fSqrDist = fA00+REAL(2.0)*fB0+fC; - } - else - { - fS = fNumer/fDenom; - fT = REAL(1.0) - fS; - fSqrDist = fS*(fA00*fS+fA01*fT+REAL(2.0)*fB0) + - fT*(fA01*fS+fA11*fT+REAL(2.0)*fB1)+fC; - } - } - else - { - fS = REAL(0.0); - if ( fTmp1 <= REAL(0.0) ) - { - fT = REAL(1.0); - fSqrDist = fA11+REAL(2.0)*fB1+fC; - } - else if ( fB1 >= REAL(0.0) ) - { - fT = REAL(0.0); - fSqrDist = fC; - } - else - { - fT = -fB1/fA11; - fSqrDist = fB1*fT+fC; - } - } - } - else if ( fT < REAL(0.0) ) // region 6 - { - fTmp0 = fA01 + fB1; - fTmp1 = fA00 + fB0; - if ( fTmp1 > fTmp0 ) - { - fNumer = fTmp1 - fTmp0; - fDenom = fA00-REAL(2.0)*fA01+fA11; - if ( fNumer >= fDenom ) - { - fT = REAL(1.0); - fS = REAL(0.0); - fSqrDist = fA11+REAL(2.0)*fB1+fC; - } - else - { - fT = fNumer/fDenom; - fS = REAL(1.0) - fT; - fSqrDist = fS*(fA00*fS+fA01*fT+REAL(2.0)*fB0) + - fT*(fA01*fS+fA11*fT+REAL(2.0)*fB1)+fC; - } - } - else - { - fT = REAL(0.0); - if ( fTmp1 <= REAL(0.0) ) - { - fS = REAL(1.0); - fSqrDist = fA00+REAL(2.0)*fB0+fC; - } - else if ( fB0 >= REAL(0.0) ) - { - fS = REAL(0.0); - fSqrDist = fC; - } - else - { - fS = -fB0/fA00; - fSqrDist = fB0*fS+fC; - } - } - } - else // region 1 - { - fNumer = fA11 + fB1 - fA01 - fB0; - if ( fNumer <= REAL(0.0) ) - { - fS = REAL(0.0); - fT = REAL(1.0); - fSqrDist = fA11+REAL(2.0)*fB1+fC; - } - else - { - fDenom = fA00-REAL(2.0)*fA01+fA11; - if ( fNumer >= fDenom ) - { - fS = REAL(1.0); - fT = REAL(0.0); - fSqrDist = fA00+REAL(2.0)*fB0+fC; - } - else - { - fS = fNumer/fDenom; - fT = REAL(1.0) - fS; - fSqrDist = fS*(fA00*fS+fA01*fT+REAL(2.0)*fB0) + - fT*(fA01*fS+fA11*fT+REAL(2.0)*fB1)+fC; - } - } - } - } - - if ( pfSParam ) - *pfSParam = (float)fS; - - if ( pfTParam ) - *pfTParam = (float)fT; - - return dReal(fabs(fSqrDist)); -} - -//------------------------------------------------------------------------------ -/** - @brief Finds the shortest distance squared between two line segments. - @param pfSegP0 t value for seg1 where the shortest distance between - the segments exists. - @param pfSegP0 t value for seg2 where the shortest distance between - the segments exists. - @return Shortest distance squared. - - Taken from: - Magic Software, Inc. - http://www.magic-software.com -*/ -dReal SqrDistanceSegments( const dVector3 seg1Origin, const dVector3 seg1Direction, - const dVector3 seg2Origin, const dVector3 seg2Direction, - dReal* pfSegP0, dReal* pfSegP1 ) -{ - const dReal gs_fTolerance = 1e-05f; - dVector3 kDiff, kNegDiff, seg1NegDirection; - dSubtractVectors3r4(kDiff, seg1Origin, seg2Origin); - Vector3Negate( kDiff, kNegDiff ); - dReal fA00 = dCalcVectorLengthSquare3( seg1Direction); - Vector3Negate( seg1Direction, seg1NegDirection ); - dReal fA01 = dCalcVectorDot3( seg1NegDirection, seg2Direction ); - dReal fA11 = dCalcVectorLengthSquare3(seg2Direction); - dReal fB0 = dCalcVectorDot3( kDiff, seg1Direction ); - dReal fC = dCalcVectorLengthSquare3(kDiff); - dReal fDet = dReal(fabs(fA00*fA11-fA01*fA01)); - dReal fB1, fS, fT, fSqrDist, fTmp; - - if ( fDet >= gs_fTolerance ) - { - // line segments are not parallel - fB1 = dCalcVectorDot3( kNegDiff, seg2Direction ); - fS = fA01*fB1-fA11*fB0; - fT = fA01*fB0-fA00*fB1; - - if ( fS >= REAL(0.0) ) - { - if ( fS <= fDet ) - { - if ( fT >= REAL(0.0) ) - { - if ( fT <= fDet ) // region 0 (interior) - { - // minimum at two interior points of 3D lines - dReal fInvDet = REAL(1.0)/fDet; - fS *= fInvDet; - fT *= fInvDet; - fSqrDist = fS*(fA00*fS+fA01*fT+REAL(2.0)*fB0) + - fT*(fA01*fS+fA11*fT+REAL(2.0)*fB1)+fC; - } - else // region 3 (side) - { - fT = REAL(1.0); - fTmp = fA01+fB0; - if ( fTmp >= REAL(0.0) ) - { - fS = REAL(0.0); - fSqrDist = fA11+REAL(2.0)*fB1+fC; - } - else if ( -fTmp >= fA00 ) - { - fS = REAL(1.0); - fSqrDist = fA00+fA11+fC+REAL(2.0)*(fB1+fTmp); - } - else - { - fS = -fTmp/fA00; - fSqrDist = fTmp*fS+fA11+REAL(2.0)*fB1+fC; - } - } - } - else // region 7 (side) - { - fT = REAL(0.0); - if ( fB0 >= REAL(0.0) ) - { - fS = REAL(0.0); - fSqrDist = fC; - } - else if ( -fB0 >= fA00 ) - { - fS = REAL(1.0); - fSqrDist = fA00+REAL(2.0)*fB0+fC; - } - else - { - fS = -fB0/fA00; - fSqrDist = fB0*fS+fC; - } - } - } - else - { - if ( fT >= REAL(0.0) ) - { - if ( fT <= fDet ) // region 1 (side) - { - fS = REAL(1.0); - fTmp = fA01+fB1; - if ( fTmp >= REAL(0.0) ) - { - fT = REAL(0.0); - fSqrDist = fA00+REAL(2.0)*fB0+fC; - } - else if ( -fTmp >= fA11 ) - { - fT = REAL(1.0); - fSqrDist = fA00+fA11+fC+REAL(2.0)*(fB0+fTmp); - } - else - { - fT = -fTmp/fA11; - fSqrDist = fTmp*fT+fA00+REAL(2.0)*fB0+fC; - } - } - else // region 2 (corner) - { - fTmp = fA01+fB0; - if ( -fTmp <= fA00 ) - { - fT = REAL(1.0); - if ( fTmp >= REAL(0.0) ) - { - fS = REAL(0.0); - fSqrDist = fA11+REAL(2.0)*fB1+fC; - } - else - { - fS = -fTmp/fA00; - fSqrDist = fTmp*fS+fA11+REAL(2.0)*fB1+fC; - } - } - else - { - fS = REAL(1.0); - fTmp = fA01+fB1; - if ( fTmp >= REAL(0.0) ) - { - fT = REAL(0.0); - fSqrDist = fA00+REAL(2.0)*fB0+fC; - } - else if ( -fTmp >= fA11 ) - { - fT = REAL(1.0); - fSqrDist = fA00+fA11+fC+REAL(2.0)*(fB0+fTmp); - } - else - { - fT = -fTmp/fA11; - fSqrDist = fTmp*fT+fA00+REAL(2.0)*fB0+fC; - } - } - } - } - else // region 8 (corner) - { - if ( -fB0 < fA00 ) - { - fT = REAL(0.0); - if ( fB0 >= REAL(0.0) ) - { - fS = REAL(0.0); - fSqrDist = fC; - } - else - { - fS = -fB0/fA00; - fSqrDist = fB0*fS+fC; - } - } - else - { - fS = REAL(1.0); - fTmp = fA01+fB1; - if ( fTmp >= REAL(0.0) ) - { - fT = REAL(0.0); - fSqrDist = fA00+REAL(2.0)*fB0+fC; - } - else if ( -fTmp >= fA11 ) - { - fT = REAL(1.0); - fSqrDist = fA00+fA11+fC+REAL(2.0)*(fB0+fTmp); - } - else - { - fT = -fTmp/fA11; - fSqrDist = fTmp*fT+fA00+REAL(2.0)*fB0+fC; - } - } - } - } - } - else - { - if ( fT >= REAL(0.0) ) - { - if ( fT <= fDet ) // region 5 (side) - { - fS = REAL(0.0); - if ( fB1 >= REAL(0.0) ) - { - fT = REAL(0.0); - fSqrDist = fC; - } - else if ( -fB1 >= fA11 ) - { - fT = REAL(1.0); - fSqrDist = fA11+REAL(2.0)*fB1+fC; - } - else - { - fT = -fB1/fA11; - fSqrDist = fB1*fT+fC; - } - } - else // region 4 (corner) - { - fTmp = fA01+fB0; - if ( fTmp < REAL(0.0) ) - { - fT = REAL(1.0); - if ( -fTmp >= fA00 ) - { - fS = REAL(1.0); - fSqrDist = fA00+fA11+fC+REAL(2.0)*(fB1+fTmp); - } - else - { - fS = -fTmp/fA00; - fSqrDist = fTmp*fS+fA11+REAL(2.0)*fB1+fC; - } - } - else - { - fS = REAL(0.0); - if ( fB1 >= REAL(0.0) ) - { - fT = REAL(0.0); - fSqrDist = fC; - } - else if ( -fB1 >= fA11 ) - { - fT = REAL(1.0); - fSqrDist = fA11+REAL(2.0)*fB1+fC; - } - else - { - fT = -fB1/fA11; - fSqrDist = fB1*fT+fC; - } - } - } - } - else // region 6 (corner) - { - if ( fB0 < REAL(0.0) ) - { - fT = REAL(0.0); - if ( -fB0 >= fA00 ) - { - fS = REAL(1.0); - fSqrDist = fA00+REAL(2.0)*fB0+fC; - } - else - { - fS = -fB0/fA00; - fSqrDist = fB0*fS+fC; - } - } - else - { - fS = REAL(0.0); - if ( fB1 >= REAL(0.0) ) - { - fT = REAL(0.0); - fSqrDist = fC; - } - else if ( -fB1 >= fA11 ) - { - fT = REAL(1.0); - fSqrDist = fA11+REAL(2.0)*fB1+fC; - } - else - { - fT = -fB1/fA11; - fSqrDist = fB1*fT+fC; - } - } - } - } - } - else - { - // line segments are parallel - if ( fA01 > REAL(0.0) ) - { - // direction vectors form an obtuse angle - if ( fB0 >= REAL(0.0) ) - { - fS = REAL(0.0); - fT = REAL(0.0); - fSqrDist = fC; - } - else if ( -fB0 <= fA00 ) - { - fS = -fB0/fA00; - fT = REAL(0.0); - fSqrDist = fB0*fS+fC; - } - else - { - //fB1 = -kDiff % seg2.m; - fB1 = dCalcVectorDot3( kNegDiff, seg2Direction ); - fS = REAL(1.0); - fTmp = fA00+fB0; - if ( -fTmp >= fA01 ) - { - fT = REAL(1.0); - fSqrDist = fA00+fA11+fC+REAL(2.0)*(fA01+fB0+fB1); - } - else - { - fT = -fTmp/fA01; - fSqrDist = fA00+REAL(2.0)*fB0+fC+fT*(fA11*fT+REAL(2.0)*(fA01+fB1)); - } - } - } - else - { - // direction vectors form an acute angle - if ( -fB0 >= fA00 ) - { - fS = REAL(1.0); - fT = REAL(0.0); - fSqrDist = fA00+REAL(2.0)*fB0+fC; - } - else if ( fB0 <= REAL(0.0) ) - { - fS = -fB0/fA00; - fT = REAL(0.0); - fSqrDist = fB0*fS+fC; - } - else - { - fB1 = dCalcVectorDot3( kNegDiff, seg2Direction ); - fS = REAL(0.0); - if ( fB0 >= -fA01 ) - { - fT = REAL(1.0); - fSqrDist = fA11+REAL(2.0)*fB1+fC; - } - else - { - fT = -fB0/fA01; - fSqrDist = fC+fT*(REAL(2.0)*fB1+fA11*fT); - } - } - } - } - - if ( pfSegP0 ) - *pfSegP0 = fS; - - if ( pfSegP1 ) - *pfSegP1 = fT; - - return dReal(fabs(fSqrDist)); -} - -//------------------------------------------------------------------------------ -/** - @brief Finds the shortest distance squared between a line segment and - a triangle. - - @param pfSegP t value for the line segment where the shortest distance between - the segment and the triangle occurs. - So the point along the segment that is the shortest distance - away from the triangle can be obtained by (seg.end - seg.start) * t. - @param pfTriP0 Barycentric coordinate of triangle at point closest to seg (u) - @param pfTriP1 Barycentric coordinate of triangle at point closest to seg (v) - @return Shortest distance squared. - - The third Barycentric coordinate is implicit, ie. w = 1.0 - u - v - - Taken from: - Magic Software, Inc. - http://www.magic-software.com -*/ -dReal SqrDistanceSegTri( const dVector3 segOrigin, const dVector3 segEnd, - const dVector3 triOrigin, - const dVector3 triEdge0, const dVector3 triEdge1, - dReal* pfSegP, dReal* pfTriP0, dReal* pfTriP1 ) -{ - const dReal gs_fTolerance = 1e-06f; - dVector3 segDirection, segNegDirection, kDiff, kNegDiff; - dSubtractVectors3r4(segDirection, segEnd, segOrigin); - Vector3Negate( segDirection, segNegDirection ); - dSubtractVectors3r4(kDiff, triOrigin, segOrigin); - Vector3Negate( kDiff, kNegDiff ); - dReal fA00 = dCalcVectorLengthSquare3(segDirection); - dReal fA01 = dCalcVectorDot3( segNegDirection, triEdge0 ); - dReal fA02 = dCalcVectorDot3( segNegDirection, triEdge1 ); - dReal fA11 = dCalcVectorLengthSquare3(triEdge0); - dReal fA12 = dCalcVectorDot3( triEdge0, triEdge1 ); - dReal fA22 = dCalcVectorLengthSquare3(triEdge1); - dReal fB0 = dCalcVectorDot3( kNegDiff, segDirection ); - dReal fB1 = dCalcVectorDot3( kDiff, triEdge0 ); - dReal fB2 = dCalcVectorDot3( kDiff, triEdge1 ); - - dVector3 kTriSegOrigin, kTriSegDirection, kPt; - dReal fSqrDist, fSqrDist0, fR, fS, fT, fR0, fS0, fT0; - - // Set up for a relative error test on the angle between ray direction - // and triangle normal to determine parallel/nonparallel status. - dVector3 kN; - dCalcVectorCross3r4( kN, triEdge0, triEdge1 ); - dReal fNSqrLen = dCalcVectorLengthSquare3(kN); - dReal fDot = dCalcVectorDot3( segDirection, kN ); - bool bNotParallel = (fDot*fDot >= gs_fTolerance*fA00*fNSqrLen); - - if ( bNotParallel ) - { - dReal fCof00 = fA11*fA22-fA12*fA12; - dReal fCof01 = fA02*fA12-fA01*fA22; - dReal fCof02 = fA01*fA12-fA02*fA11; - dReal fCof11 = fA00*fA22-fA02*fA02; - dReal fCof12 = fA02*fA01-fA00*fA12; - dReal fCof22 = fA00*fA11-fA01*fA01; - dReal fInvDet = REAL(1.0)/(fA00*fCof00+fA01*fCof01+fA02*fCof02); - dReal fRhs0 = -fB0*fInvDet; - dReal fRhs1 = -fB1*fInvDet; - dReal fRhs2 = -fB2*fInvDet; - - fR = fCof00*fRhs0+fCof01*fRhs1+fCof02*fRhs2; - fS = fCof01*fRhs0+fCof11*fRhs1+fCof12*fRhs2; - fT = fCof02*fRhs0+fCof12*fRhs1+fCof22*fRhs2; - - if ( fR < REAL(0.0) ) - { - if ( fS+fT <= REAL(1.0) ) - { - if ( fS < REAL(0.0) ) - { - if ( fT < REAL(0.0) ) // region 4m - { - // min on face s=0 or t=0 or r=0 - Vector3Copy( triOrigin, kTriSegOrigin ); - Vector3Copy( triEdge1, kTriSegDirection ); - fSqrDist = SqrDistanceSegments( segOrigin, segDirection, - kTriSegOrigin, kTriSegDirection, - &fR, &fT ); - fS = REAL(0.0); - Vector3Copy( triOrigin, kTriSegOrigin ); - Vector3Copy( triEdge0, kTriSegDirection ); - fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection, - kTriSegOrigin, kTriSegDirection, - &fR0, &fS0 ); - fT0 = REAL(0.0); - if ( fSqrDist0 < fSqrDist ) - { - fSqrDist = fSqrDist0; - fR = fR0; - fS = fS0; - fT = fT0; - } - fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1, - &fS0, &fT0 ); - fR0 = REAL(0.0); - if ( fSqrDist0 < fSqrDist ) - { - fSqrDist = fSqrDist0; - fR = fR0; - fS = fS0; - fT = fT0; - } - } - else // region 3m - { - // min on face s=0 or r=0 - Vector3Copy( triOrigin, kTriSegOrigin ); - Vector3Copy( triEdge1, kTriSegDirection ); - fSqrDist = SqrDistanceSegments( segOrigin, segDirection, - kTriSegOrigin, kTriSegDirection, - &fR,&fT ); - fS = REAL(0.0); - fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1, - &fS0, &fT0 ); - fR0 = REAL(0.0); - if ( fSqrDist0 < fSqrDist ) - { - fSqrDist = fSqrDist0; - fR = fR0; - fS = fS0; - fT = fT0; - } - } - } - else if ( fT < REAL(0.0) ) // region 5m - { - // min on face t=0 or r=0 - Vector3Copy( triOrigin, kTriSegOrigin ); - Vector3Copy( triEdge0, kTriSegDirection ); - fSqrDist = SqrDistanceSegments( segOrigin, segDirection, - kTriSegOrigin, kTriSegDirection, - &fR, &fS ); - fT = REAL(0.0); - fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1, - &fS0, &fT0 ); - fR0 = REAL(0.0); - if ( fSqrDist0 < fSqrDist ) - { - fSqrDist = fSqrDist0; - fR = fR0; - fS = fS0; - fT = fT0; - } - } - else // region 0m - { - // min on face r=0 - fSqrDist = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1, - &fS, &fT ); - fR = REAL(0.0); - } - } - else - { - if ( fS < REAL(0.0) ) // region 2m - { - // min on face s=0 or s+t=1 or r=0 - Vector3Copy( triOrigin, kTriSegOrigin ); - Vector3Copy( triEdge1, kTriSegDirection ); - fSqrDist = SqrDistanceSegments( segOrigin, segDirection, - kTriSegOrigin, kTriSegDirection, - &fR, &fT ); - fS = REAL(0.0); - dAddVectors3r4(kTriSegOrigin, triOrigin, triEdge0); - dSubtractVectors3r4(kTriSegDirection, triEdge1, triEdge0); - fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection, - kTriSegOrigin, kTriSegDirection, - &fR0, &fT0 ); - fS0 = REAL(1.0) - fT0; - if ( fSqrDist0 < fSqrDist ) - { - fSqrDist = fSqrDist0; - fR = fR0; - fS = fS0; - fT = fT0; - } - fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1, - &fS0, &fT0 ); - fR0 = REAL(0.0); - if ( fSqrDist0 < fSqrDist ) - { - fSqrDist = fSqrDist0; - fR = fR0; - fS = fS0; - fT = fT0; - } - } - else if ( fT < REAL(0.0) ) // region 6m - { - // min on face t=0 or s+t=1 or r=0 - Vector3Copy( triOrigin, kTriSegOrigin ); - Vector3Copy( triEdge0, kTriSegDirection ); - fSqrDist = SqrDistanceSegments( segOrigin, segDirection, - kTriSegOrigin, kTriSegDirection, - &fR, &fS ); - fT = REAL(0.0); - dAddVectors3r4(kTriSegOrigin, triOrigin, triEdge0); - dSubtractVectors3r4(kTriSegDirection, triEdge1, triEdge0); - fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection, - kTriSegOrigin, kTriSegDirection, - &fR0, &fT0 ); - fS0 = REAL(1.0) - fT0; - if ( fSqrDist0 < fSqrDist ) - { - fSqrDist = fSqrDist0; - fR = fR0; - fS = fS0; - fT = fT0; - } - fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1, - &fS0, &fT0 ); - fR0 = REAL(0.0); - if ( fSqrDist0 < fSqrDist ) - { - fSqrDist = fSqrDist0; - fR = fR0; - fS = fS0; - fT = fT0; - } - } - else // region 1m - { - // min on face s+t=1 or r=0 - dAddVectors3r4(kTriSegOrigin, triOrigin, triEdge0); - dSubtractVectors3r4(kTriSegDirection, triEdge1, triEdge0); - fSqrDist = SqrDistanceSegments( segOrigin, segDirection, - kTriSegOrigin, kTriSegDirection, - &fR, &fT ); - fS = REAL(1.0) - fT; - fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1, - &fS0, &fT0 ); - fR0 = REAL(0.0); - if ( fSqrDist0 < fSqrDist ) - { - fSqrDist = fSqrDist0; - fR = fR0; - fS = fS0; - fT = fT0; - } - } - } - } - else if ( fR <= REAL(1.0) ) - { - if ( fS+fT <= REAL(1.0) ) - { - if ( fS < REAL(0.0) ) - { - if ( fT < REAL(0.0) ) // region 4 - { - // min on face s=0 or t=0 - Vector3Copy( triOrigin, kTriSegOrigin ); - Vector3Copy( triEdge1, kTriSegDirection ); - fSqrDist = SqrDistanceSegments( segOrigin, segDirection, - kTriSegOrigin, kTriSegDirection, - &fR, &fT ); - fS = REAL(0.0); - Vector3Copy( triOrigin, kTriSegOrigin ); - Vector3Copy( triEdge0, kTriSegDirection ); - fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection, - kTriSegOrigin, kTriSegDirection, - &fR0, &fS0 ); - fT0 = REAL(0.0); - if ( fSqrDist0 < fSqrDist ) - { - fSqrDist = fSqrDist0; - fR = fR0; - fS = fS0; - fT = fT0; - } - } - else // region 3 - { - // min on face s=0 - Vector3Copy( triOrigin, kTriSegOrigin ); - Vector3Copy( triEdge1, kTriSegDirection ); - fSqrDist = SqrDistanceSegments( segOrigin, segDirection, - kTriSegOrigin, kTriSegDirection, - &fR, &fT ); - fS = REAL(0.0); - } - } - else if ( fT < REAL(0.0) ) // region 5 - { - // min on face t=0 - Vector3Copy( triOrigin, kTriSegOrigin ); - Vector3Copy( triEdge0, kTriSegDirection ); - fSqrDist = SqrDistanceSegments( segOrigin, segDirection, - kTriSegOrigin, kTriSegDirection, - &fR, &fS ); - fT = REAL(0.0); - } - else // region 0 - { - // global minimum is interior, done - fSqrDist = REAL(0.0); - } - } - else - { - if ( fS < REAL(0.0) ) // region 2 - { - // min on face s=0 or s+t=1 - Vector3Copy( triOrigin, kTriSegOrigin ); - Vector3Copy( triEdge1, kTriSegDirection ); - fSqrDist = SqrDistanceSegments( segOrigin, segDirection, - kTriSegOrigin, kTriSegDirection, - &fR, &fT ); - fS = REAL(0.0); - dAddVectors3r4(kTriSegOrigin, triOrigin, triEdge0); - dSubtractVectors3r4(kTriSegDirection, triEdge1, triEdge0); - fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection, - kTriSegOrigin, kTriSegDirection, - &fR0, &fT0 ); - fS0 = REAL(1.0) - fT0; - if ( fSqrDist0 < fSqrDist ) - { - fSqrDist = fSqrDist0; - fR = fR0; - fS = fS0; - fT = fT0; - } - } - else if ( fT < REAL(0.0) ) // region 6 - { - // min on face t=0 or s+t=1 - Vector3Copy( triOrigin, kTriSegOrigin ); - Vector3Copy( triEdge0, kTriSegDirection ); - fSqrDist = SqrDistanceSegments( segOrigin, segDirection, - kTriSegOrigin, kTriSegDirection, - &fR, &fS ); - fT = REAL(0.0); - dAddVectors3r4(kTriSegOrigin, triOrigin, triEdge0); - dSubtractVectors3r4( kTriSegDirection, triEdge1, triEdge0); - fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection, - kTriSegOrigin, kTriSegDirection, - &fR0, &fT0 ); - fS0 = REAL(1.0) - fT0; - if ( fSqrDist0 < fSqrDist ) - { - fSqrDist = fSqrDist0; - fR = fR0; - fS = fS0; - fT = fT0; - } - } - else // region 1 - { - // min on face s+t=1 - dAddVectors3r4(kTriSegOrigin, triOrigin, triEdge0); - dSubtractVectors3r4(kTriSegDirection, triEdge1, triEdge0); - fSqrDist = SqrDistanceSegments( segOrigin, segDirection, - kTriSegOrigin, kTriSegDirection, - &fR, &fT ); - fS = REAL(1.0) - fT; - } - } - } - else // fR > 1 - { - if ( fS+fT <= REAL(1.0) ) - { - if ( fS < REAL(0.0) ) - { - if ( fT < REAL(0.0) ) // region 4p - { - // min on face s=0 or t=0 or r=1 - Vector3Copy( triOrigin, kTriSegOrigin ); - Vector3Copy( triEdge1, kTriSegDirection ); - fSqrDist = SqrDistanceSegments( segOrigin, segDirection, - kTriSegOrigin, kTriSegDirection, - &fR, &fT ); - fS = REAL(0.0); - Vector3Copy( triOrigin, kTriSegOrigin ); - Vector3Copy( triEdge0, kTriSegDirection ); - fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection, - kTriSegOrigin, kTriSegDirection, - &fR0, &fS0 ); - fT0 = REAL(0.0); - if ( fSqrDist0 < fSqrDist ) - { - fSqrDist = fSqrDist0; - fR = fR0; - fS = fS0; - fT = fT0; - } - dAddVectors3r4( kPt, segOrigin, segDirection); - fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1, - &fS0, &fT0 ); - fR0 = REAL(1.0); - if ( fSqrDist0 < fSqrDist ) - { - fSqrDist = fSqrDist0; - fR = fR0; - fS = fS0; - fT = fT0; - } - } - else // region 3p - { - // min on face s=0 or r=1 - Vector3Copy( triOrigin, kTriSegOrigin ); - Vector3Copy( triEdge1, kTriSegDirection ); - fSqrDist = SqrDistanceSegments( segOrigin, segDirection, - kTriSegOrigin, kTriSegDirection, - &fR, &fT ); - fS = REAL(0.0); - dAddVectors3r4(kPt, segOrigin, segDirection); - fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1, - &fS0, &fT0 ); - fR0 = REAL(1.0); - if ( fSqrDist0 < fSqrDist ) - { - fSqrDist = fSqrDist0; - fR = fR0; - fS = fS0; - fT = fT0; - } - } - } - else if ( fT < REAL(0.0) ) // region 5p - { - // min on face t=0 or r=1 - Vector3Copy( triOrigin, kTriSegOrigin ); - Vector3Copy( triEdge0, kTriSegDirection ); - fSqrDist = SqrDistanceSegments( segOrigin, segDirection, - kTriSegOrigin, kTriSegDirection, - &fR, &fS ); - fT = REAL(0.0); - dAddVectors3r4(kPt, segOrigin, segDirection); - fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1, - &fS0, &fT0 ); - fR0 = REAL(1.0); - if ( fSqrDist0 < fSqrDist ) - { - fSqrDist = fSqrDist0; - fR = fR0; - fS = fS0; - fT = fT0; - } - } - else // region 0p - { - // min face on r=1 - dAddVectors3r4(kPt, segOrigin, segDirection); - fSqrDist = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1, - &fS, &fT ); - fR = REAL(1.0); - } - } - else - { - if ( fS < REAL(0.0) ) // region 2p - { - // min on face s=0 or s+t=1 or r=1 - Vector3Copy( triOrigin, kTriSegOrigin ); - Vector3Copy( triEdge1, kTriSegDirection ); - fSqrDist = SqrDistanceSegments( segOrigin, segDirection, - kTriSegOrigin, kTriSegDirection, - &fR, &fT ); - fS = REAL(0.0); - dAddVectors3r4(kTriSegOrigin, triOrigin, triEdge0); - dSubtractVectors3r4(kTriSegDirection, triEdge1, triEdge0); - fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection, - kTriSegOrigin, kTriSegDirection, - &fR0, &fT0 ); - fS0 = REAL(1.0) - fT0; - if ( fSqrDist0 < fSqrDist ) - { - fSqrDist = fSqrDist0; - fR = fR0; - fS = fS0; - fT = fT0; - } - dAddVectors3r4(kPt, segOrigin, segDirection); - fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1, - &fS0, &fT0 ); - fR0 = REAL(1.0); - if ( fSqrDist0 < fSqrDist ) - { - fSqrDist = fSqrDist0; - fR = fR0; - fS = fS0; - fT = fT0; - } - } - else if ( fT < REAL(0.0) ) // region 6p - { - // min on face t=0 or s+t=1 or r=1 - Vector3Copy( triOrigin, kTriSegOrigin ); - Vector3Copy( triEdge0, kTriSegDirection ); - fSqrDist = SqrDistanceSegments( segOrigin, segDirection, - kTriSegOrigin, kTriSegDirection, - &fR, &fS ); - fT = REAL(0.0); - dAddVectors3r4(kTriSegOrigin, triOrigin, triEdge0); - dSubtractVectors3r4(kTriSegDirection, triEdge1, triEdge0); - fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection, - kTriSegOrigin, kTriSegDirection, - &fR0, &fT0 ); - fS0 = REAL(1.0) - fT0; - if ( fSqrDist0 < fSqrDist ) - { - fSqrDist = fSqrDist0; - fR = fR0; - fS = fS0; - fT = fT0; - } - dAddVectors3r4(kPt, segOrigin, segDirection); - fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1, - &fS0, &fT0 ); - fR0 = REAL(1.0); - if ( fSqrDist0 < fSqrDist ) - { - fSqrDist = fSqrDist0; - fR = fR0; - fS = fS0; - fT = fT0; - } - } - else // region 1p - { - // min on face s+t=1 or r=1 - dAddVectors3r4(kTriSegOrigin, triOrigin, triEdge0); - dSubtractVectors3r4(kTriSegDirection, triEdge1, triEdge0); - fSqrDist = SqrDistanceSegments( segOrigin, segDirection, - kTriSegOrigin, kTriSegDirection, - &fR, &fT ); - fS = REAL(1.0) - fT; - dAddVectors3r4(kPt, segOrigin, segDirection); - fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1, - &fS0, &fT0 ); - fR0 = REAL(1.0); - if ( fSqrDist0 < fSqrDist ) - { - fSqrDist = fSqrDist0; - fR = fR0; - fS = fS0; - fT = fT0; - } - } - } - } - } - else - { - // segment and triangle are parallel - Vector3Copy( triOrigin, kTriSegOrigin ); - Vector3Copy( triEdge0, kTriSegDirection ); - fSqrDist = SqrDistanceSegments( segOrigin, segDirection, - kTriSegOrigin, kTriSegDirection, &fR, &fS ); - fT = REAL(0.0); - - Vector3Copy( triEdge1, kTriSegDirection ); - fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection, - kTriSegOrigin, kTriSegDirection, - &fR0, &fT0 ); - fS0 = REAL(0.0); - if ( fSqrDist0 < fSqrDist ) - { - fSqrDist = fSqrDist0; - fR = fR0; - fS = fS0; - fT = fT0; - } - - dAddVectors3r4(kTriSegOrigin, triOrigin, triEdge0); - dSubtractVectors3r4(kTriSegDirection, triEdge1, triEdge0); - fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection, - kTriSegOrigin, kTriSegDirection, &fR0, &fT0 ); - fS0 = REAL(1.0) - fT0; - if ( fSqrDist0 < fSqrDist ) - { - fSqrDist = fSqrDist0; - fR = fR0; - fS = fS0; - fT = fT0; - } - - fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1, - &fS0, &fT0 ); - fR0 = REAL(0.0); - if ( fSqrDist0 < fSqrDist ) - { - fSqrDist = fSqrDist0; - fR = fR0; - fS = fS0; - fT = fT0; - } - - dAddVectors3r4(kPt, segOrigin, segDirection); - fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1, - &fS0, &fT0 ); - fR0 = REAL(1.0); - if ( fSqrDist0 < fSqrDist ) - { - fSqrDist = fSqrDist0; - fR = fR0; - fS = fS0; - fT = fT0; - } - } - - if ( pfSegP ) - *pfSegP = fR; - - if ( pfTriP0 ) - *pfTriP0 = fS; - - if ( pfTriP1 ) - *pfTriP1 = fT; - - return fSqrDist; -} diff --git a/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_internal.h b/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_internal.h index 4e0d3b51..b0c8f5e0 100644 --- a/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_internal.h +++ b/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_internal.h @@ -186,7 +186,10 @@ struct dxTriMeshData : public dBase kVert0 = 0x8, kVert1 = 0x10, kVert2 = 0x20, - kUseAll = 0x3F + kUseAll = 0x3F, + kvert0or1 = kVert0 | kVert1, + kvert1or2 = kVert1 | kVert2, + kvert0or2 = kVert0 | kVert2 }; enum MeshFlags @@ -208,17 +211,15 @@ struct dxTriMeshData : public dBase dxTriMeshData(); ~dxTriMeshData(); - void Build(const void* Vertices, int VertexStide, int VertexCount, - const void* Indices, int IndexCount, int TriStride, - const void* Normals, - bool Single); + void Build(const void* Vertices, int VertexCount, + const void* Indices, int IndexCount); /* aabb in model space */ dVector3 AABBCenter; dVector3 AABBExtents; // data for use in collision resolution - const void* Normals; + //const void* Normals; uint8* UseFlags; }; @@ -293,35 +294,23 @@ inline unsigned FetchTriangleCount(dxTriMesh* TriMesh) inline void FetchTriangle(dxTriMesh* TriMesh, int Index, const dVector3 Position, const dMatrix3 Rotation, dVector3 Out[3]) { VertexPointers VP; - ConversionArea VC; - TriMesh->Data->Mesh.GetTriangle(VP, Index, VC); + TriMesh->Data->Mesh.GetTriangle(VP, Index); - /* - for (int i = 0; i < 3; i++) - { - dVector3 v; - v[0] = VP.Vertex[i]->x; - v[1] = VP.Vertex[i]->y; - v[2] = VP.Vertex[i]->z; - - dPointRotateTrans_r4(Out[i], Rotation, v, Position); - } - */ dVector3 invecs[3]; - for (int i = 0; i < 3; i++) { invecs[i][0] = VP.Vertex[i]->x; invecs[i][1] = VP.Vertex[i]->y; invecs[i][2] = VP.Vertex[i]->z; } - dtriangleRotateTrans_r4(Out, invecs, Rotation, Position); } -inline void FetchTransformedTriangle(dxTriMesh* TriMesh, int Index, dVector3 Out[3]){ - const dVector3& Position = *(const dVector3*)dGeomGetPosition(TriMesh); - const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(TriMesh); +inline void FetchTransformedTriangle(dxTriMesh* TriMesh, int Index, dVector3 Out[3]) +{ + dxPosR* dpr = TriMesh->GetRecomputePosR(); + const dVector3& Position = *(const dVector3*)dpr->pos; + const dMatrix3& Rotation = *(const dMatrix3*)dpr->R; FetchTriangle(TriMesh, Index, Position, Rotation, Out); } @@ -351,8 +340,9 @@ inline Matrix4x4& MakeMatrix(const dVector3 Position, const dMatrix3 Rotation, M } inline Matrix4x4& MakeMatrix(dxGeom* g, Matrix4x4& Out){ - const dVector3& Position = *(const dVector3*)dGeomGetPosition(g); - const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(g); + dxPosR* dpr = g->GetRecomputePosR(); + const dVector3& Position = *(const dVector3*)dpr->pos; + const dMatrix3& Rotation = *(const dMatrix3*)dpr->R; return MakeMatrix(Position, Rotation, Out); } @@ -404,79 +394,4 @@ template const T& dcMIN(const T& x, const T& y){ return x < y ? x : y; } -dReal SqrDistancePointTri( const dVector3 p, const dVector3 triOrigin, - const dVector3 triEdge1, const dVector3 triEdge2, - dReal* pfSParam = 0, dReal* pfTParam = 0 ); - -dReal SqrDistanceSegments( const dVector3 seg1Origin, const dVector3 seg1Direction, - const dVector3 seg2Origin, const dVector3 seg2Direction, - dReal* pfSegP0 = 0, dReal* pfSegP1 = 0 ); - -dReal SqrDistanceSegTri( const dVector3 segOrigin, const dVector3 segEnd, - const dVector3 triOrigin, - const dVector3 triEdge1, const dVector3 triEdge2, - dReal* t = 0, dReal* u = 0, dReal* v = 0 ); - -inline -void Vector3Negate( const dVector3 in, dVector3 out ) -{ - out[0] = -in[0]; - out[1] = -in[1]; - out[2] = -in[2]; - out[3] = REAL(0.0); -} - -inline -void Vector3Copy( const dVector3 in, dVector3 out ) -{ - out[0] = in[0]; - out[1] = in[1]; - out[2] = in[2]; - out[3] = REAL(0.0); -} - -inline -void Vector3Multiply( const dVector3 in, dReal scalar, dVector3 out ) -{ - out[0] = in[0] * scalar; - out[1] = in[1] * scalar; - out[2] = in[2] * scalar; - out[3] = REAL(0.0); -} - -//------------------------------------------------------------------------------ -/** - @brief Check for intersection between triangle and capsule. - - @param dist [out] Shortest distance squared between the triangle and - the capsule segment (central axis). - @param t [out] t value of point on segment that's the shortest distance - away from the triangle, the coordinates of this point - can be found by (cap.seg.end - cap.seg.start) * t, - or cap.seg.ipol(t). - @param u [out] Barycentric coord on triangle. - @param v [out] Barycentric coord on triangle. - @return True if intersection exists. - - The third Barycentric coord is implicit, ie. w = 1.0 - u - v - The Barycentric coords give the location of the point on the triangle - closest to the capsule (where the distance between the two shapes - is the shortest). -*/ -inline -bool IntersectCapsuleTri( const dVector3 segOrigin, const dVector3 segEnd, - const dReal radius, const dVector3 triOrigin, - const dVector3 triEdge0, const dVector3 triEdge1, - dReal* dist, dReal* t, dReal* u, dReal* v ) -{ - dReal sqrDist = SqrDistanceSegTri( segOrigin, segEnd, triOrigin, triEdge0, triEdge1, - t, u, v ); - - if ( dist ) - *dist = sqrDist; - - return ( sqrDist <= (radius * radius) ); -} - - #endif //_ODE_COLLISION_TRIMESH_INTERNAL_H_ diff --git a/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_opcode.cpp b/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_opcode.cpp index fdced284..3736d62a 100644 --- a/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_opcode.cpp +++ b/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_opcode.cpp @@ -62,8 +62,6 @@ void TrimeshCollidersCache::InitOPCODECaches() */ } - - // Trimesh data dxTriMeshData::dxTriMeshData() : UseFlags( NULL ) { @@ -76,16 +74,15 @@ dxTriMeshData::~dxTriMeshData() } void -dxTriMeshData::Build(const void* Vertices, int VertexStide, int VertexCount, - const void* Indices, int IndexCount, int TriStride, - const void* in_Normals, - bool Single) +dxTriMeshData::Build(const void* Vertices,int VertexCount, + const void* Indices, int IndexCount) { Mesh.SetNbTriangles(IndexCount / 3); Mesh.SetNbVertices(VertexCount); Mesh.SetPointers((IndexedTriangle*)Indices, (Point*)Vertices); - Mesh.SetStrides(TriStride, VertexStide); - Mesh.SetSingle(Single); + Mesh.SetSingle(true); + + int vertStride = Mesh.GetVertexStride(); // Build tree BuildSettings Settings; @@ -101,9 +98,6 @@ dxTriMeshData::Build(const void* Vertices, int VertexStide, int VertexCount, TreeBuilder.mIMesh = &Mesh; TreeBuilder.mSettings = Settings; - TreeBuilder.mNoLeaf = true; - TreeBuilder.mQuantized = false; -// TreeBuilder.mQuantized = true; TreeBuilder.mKeepOriginal = false; TreeBuilder.mCanRemap = false; @@ -111,43 +105,41 @@ dxTriMeshData::Build(const void* Vertices, int VertexStide, int VertexCount, BVTree.Build(TreeBuilder); // compute model space AABB - dVector3 AABBMax, AABBMin; - AABBMax[0] = AABBMax[1] = AABBMax[2] = (dReal) -dInfinity; - AABBMin[0] = AABBMin[1] = AABBMin[2] = (dReal) dInfinity; - if( Single ) { - const char* verts = (const char*)Vertices; - for( int i = 0; i < VertexCount; ++i ) { - const float* v = (const float*)verts; - if( v[0] > AABBMax[0] ) AABBMax[0] = v[0]; - if( v[1] > AABBMax[1] ) AABBMax[1] = v[1]; - if( v[2] > AABBMax[2] ) AABBMax[2] = v[2]; - if( v[0] < AABBMin[0] ) AABBMin[0] = v[0]; - if( v[1] < AABBMin[1] ) AABBMin[1] = v[1]; - if( v[2] < AABBMin[2] ) AABBMin[2] = v[2]; - verts += VertexStide; - } - } else { - const char* verts = (const char*)Vertices; - for( int i = 0; i < VertexCount; ++i ) { - const double* v = (const double*)verts; - if( v[0] > AABBMax[0] ) AABBMax[0] = (dReal) v[0]; - if( v[1] > AABBMax[1] ) AABBMax[1] = (dReal) v[1]; - if( v[2] > AABBMax[2] ) AABBMax[2] = (dReal) v[2]; - if( v[0] < AABBMin[0] ) AABBMin[0] = (dReal) v[0]; - if( v[1] < AABBMin[1] ) AABBMin[1] = (dReal) v[1]; - if( v[2] < AABBMin[2] ) AABBMin[2] = (dReal) v[2]; - verts += VertexStide; - } - } - AABBCenter[0] = (AABBMin[0] + AABBMax[0]) * REAL(0.5); - AABBCenter[1] = (AABBMin[1] + AABBMax[1]) * REAL(0.5); - AABBCenter[2] = (AABBMin[2] + AABBMax[2]) * REAL(0.5); - AABBExtents[0] = AABBMax[0] - AABBCenter[0]; - AABBExtents[1] = AABBMax[1] - AABBCenter[1]; - AABBExtents[2] = AABBMax[2] - AABBCenter[2]; + const char* verts = (const char*)Vertices; +#if(__AVX__) + const __m128 half = _mm_set1_ps(0.5f); + __m128 ma, mb, mc; - // user data (not used by OPCODE) - Normals = (dReal *) in_Normals; + ma = _mm_set1_ps(dInfinity); + mb = _mm_set1_ps(-dInfinity); + + for (int i = 0; i < VertexCount; ++i) + { + mc = _mm_loadu_ps((const dReal*)verts); + ma = _mm_min_ps(ma, mc); + mb = _mm_max_ps(mb, mc); + verts += vertStride; + } + + ma = _mm_add_ps(ma, mb); + ma = _mm_mul_ps(ma, half); + _mm_storeu_ps(AABBCenter, ma); + + ma = _mm_sub_ps(mb, ma); + _mm_storeu_ps(AABBExtents, ma); +#else + dVector3 AABBMax, AABBMin; + AABBMin[0] = AABBMin[1] = AABBMin[2] = (dReal)dInfinity; + AABBMax[0] = AABBMax[1] = AABBMax[2] = (dReal)-dInfinity; + for (int i = 0; i < VertexCount; ++i) + { + dMinMaxVectors3r4(AABBMin, AABBMax, (const dReal*)verts); + verts += vertStride; + } + + dAvgVectors3r4(AABBCenter, AABBMin, AABBMax); + dSubtractVectors3(AABBExtents, AABBMax, AABBCenter); +#endif UseFlags = 0; } @@ -176,45 +168,69 @@ static int EdgeCompare(const void* edge1, const void* edge2) return e1->VertIdx1 - e2->VertIdx1; } -void SetupEdge(EdgeRecord* edge, int edgeIdx, int triIdx, const dTriIndex* vertIdxs) +void SetupEdge(EdgeRecord* edge, int edgeIdx, const int triIdx, const dTriIndex* vertIdxs) { - if (edgeIdx == 0) + switch (edgeIdx) { - edge->EdgeFlags = dxTriMeshData::kEdge0; - edge->Vert1Flags = dxTriMeshData::kVert0; - edge->Vert2Flags = dxTriMeshData::kVert1; - edge->VertIdx1 = vertIdxs[0]; - edge->VertIdx2 = vertIdxs[1]; + case 0: + { + edge->EdgeFlags = dxTriMeshData::kEdge0; + // Make sure vert index 1 is less than index 2 (for easier sorting) + if (vertIdxs[0] > vertIdxs[1]) + { + edge->Vert1Flags = dxTriMeshData::kVert1; + edge->Vert2Flags = dxTriMeshData::kVert0; + edge->VertIdx1 = vertIdxs[1]; + edge->VertIdx2 = vertIdxs[0]; + } + else + { + edge->Vert1Flags = dxTriMeshData::kVert0; + edge->Vert2Flags = dxTriMeshData::kVert1; + edge->VertIdx1 = vertIdxs[0]; + edge->VertIdx2 = vertIdxs[1]; + } + break; + } + case 1: + { + edge->EdgeFlags = dxTriMeshData::kEdge1; + if (vertIdxs[1] > vertIdxs[2]) + { + edge->Vert1Flags = dxTriMeshData::kVert2; + edge->Vert2Flags = dxTriMeshData::kVert1; + edge->VertIdx1 = vertIdxs[2]; + edge->VertIdx2 = vertIdxs[1]; + } + else + { + edge->Vert1Flags = dxTriMeshData::kVert1; + edge->Vert2Flags = dxTriMeshData::kVert2; + edge->VertIdx1 = vertIdxs[1]; + edge->VertIdx2 = vertIdxs[2]; + } + break; + } + default: + { + edge->EdgeFlags = dxTriMeshData::kEdge2; + if (vertIdxs[2] > vertIdxs[0]) + { + edge->Vert1Flags = dxTriMeshData::kVert0; + edge->Vert2Flags = dxTriMeshData::kVert2; + edge->VertIdx1 = vertIdxs[0]; + edge->VertIdx2 = vertIdxs[2]; + } + else + { + edge->Vert1Flags = dxTriMeshData::kVert2; + edge->Vert2Flags = dxTriMeshData::kVert0; + edge->VertIdx1 = vertIdxs[2]; + edge->VertIdx2 = vertIdxs[0]; + } + break; + } } - else if (edgeIdx == 1) - { - edge->EdgeFlags = dxTriMeshData::kEdge1; - edge->Vert1Flags = dxTriMeshData::kVert1; - edge->Vert2Flags = dxTriMeshData::kVert2; - edge->VertIdx1 = vertIdxs[1]; - edge->VertIdx2 = vertIdxs[2]; - } - else if (edgeIdx == 2) - { - edge->EdgeFlags = dxTriMeshData::kEdge2; - edge->Vert1Flags = dxTriMeshData::kVert2; - edge->Vert2Flags = dxTriMeshData::kVert0; - edge->VertIdx1 = vertIdxs[2]; - edge->VertIdx2 = vertIdxs[0]; - } - - // Make sure vert index 1 is less than index 2 (for easier sorting) - if (edge->VertIdx1 > edge->VertIdx2) - { - unsigned int tempIdx = edge->VertIdx1; - edge->VertIdx1 = edge->VertIdx2; - edge->VertIdx2 = tempIdx; - - uint8 tempFlags = edge->Vert1Flags; - edge->Vert1Flags = edge->Vert2Flags; - edge->Vert2Flags = tempFlags; - } - edge->TriIdx = triIdx; edge->Concave = false; } @@ -222,13 +238,12 @@ void SetupEdge(EdgeRecord* edge, int edgeIdx, int triIdx, const dTriIndex* vertI // Get the vertex opposite this edge in the triangle inline Point GetOppositeVert(EdgeRecord* edge, const Point* vertices[]) { - if ((edge->Vert1Flags == dxTriMeshData::kVert0 && edge->Vert2Flags == dxTriMeshData::kVert1) || - (edge->Vert1Flags == dxTriMeshData::kVert1 && edge->Vert2Flags == dxTriMeshData::kVert0)) + uint8 curvflags = edge->Vert1Flags | edge->Vert2Flags; + if ((curvflags & dxTriMeshData::kvert0or1) == dxTriMeshData::kvert0or1) { return *vertices[2]; } - else if ((edge->Vert1Flags == dxTriMeshData::kVert1 && edge->Vert2Flags == dxTriMeshData::kVert2) || - (edge->Vert1Flags == dxTriMeshData::kVert2 && edge->Vert2Flags == dxTriMeshData::kVert1)) + else if ((curvflags & dxTriMeshData::kvert1or2) == dxTriMeshData::kvert1or2) { return *vertices[0]; } @@ -238,7 +253,6 @@ inline Point GetOppositeVert(EdgeRecord* edge, const Point* vertices[]) void dxTriMeshData::Preprocess() { - // If this mesh has already been preprocessed, exit if (UseFlags) return; @@ -284,8 +298,7 @@ void dxTriMeshData::Preprocess() rec1->VertIdx2 == rec2->VertIdx2) { VertexPointers vp; - ConversionArea vc; - Mesh.GetTriangle(vp, rec1->TriIdx, vc); + Mesh.GetTriangle(vp, rec1->TriIdx); // Get the normal of the first triangle Point triNorm = (*vp.Vertex[2] - *vp.Vertex[1]) ^ (*vp.Vertex[0] - *vp.Vertex[1]); @@ -295,7 +308,7 @@ void dxTriMeshData::Preprocess() Point oppositeVert1 = GetOppositeVert(rec1, vp.Vertex); // Get the vert opposite this edge in the second triangle - Mesh.GetTriangle(vp, rec2->TriIdx, vc); + Mesh.GetTriangle(vp, rec2->TriIdx); Point oppositeVert2 = GetOppositeVert(rec2, vp.Vertex); float dot = triNorm.Dot((oppositeVert2 - oppositeVert1).Normalize()); @@ -363,9 +376,6 @@ void dGeomTriMeshDataDestroy(dTriMeshDataID g){ delete g; } - - - void dGeomTriMeshSetLastTransform( dxGeom* g, dMatrix4 last_trans ) { dAASSERT(g); @@ -377,7 +387,6 @@ void dGeomTriMeshSetLastTransform( dxGeom* g, dMatrix4 last_trans ) return; } - dReal* dGeomTriMeshGetLastTransform( dxGeom* g ) { dAASSERT(g); @@ -386,24 +395,9 @@ dReal* dGeomTriMeshGetLastTransform( dxGeom* g ) return (dReal*)(((dxTriMesh*)g)->last_trans); } - - - void dGeomTriMeshDataSet(dTriMeshDataID g, int data_id, void* in_data) { dUASSERT(g, "argument not trimesh data"); - - switch (data_id) - { - case TRIMESH_FACE_NORMALS: - g->Normals = (dReal *) in_data; - break; - - default: - dUASSERT(data_id, "invalid data type"); - break; - } - return; } @@ -412,18 +406,6 @@ void dGeomTriMeshDataSet(dTriMeshDataID g, int data_id, void* in_data) void* dGeomTriMeshDataGet(dTriMeshDataID g, int data_id) { dUASSERT(g, "argument not trimesh data"); - - switch (data_id) - { - case TRIMESH_FACE_NORMALS: - return (void *) g->Normals; - break; - - default: - dUASSERT(data_id, "invalid data type"); - break; - } - return NULL; } @@ -435,72 +417,47 @@ void dGeomTriMeshDataBuildSingle1(dTriMeshDataID g, { dUASSERT(g, "argument not trimesh data"); - g->Build(Vertices, VertexStride, VertexCount, - Indices, IndexCount, TriStride, - Normals, - true); + g->Build(Vertices, VertexCount, Indices, IndexCount); } - void dGeomTriMeshDataBuildSingle(dTriMeshDataID g, const void* Vertices, int VertexStride, int VertexCount, const void* Indices, int IndexCount, int TriStride) { - dGeomTriMeshDataBuildSingle1(g, Vertices, VertexStride, VertexCount, - Indices, IndexCount, TriStride, (void*)NULL); + g->Build(Vertices, VertexCount,Indices, IndexCount); } - -void dGeomTriMeshDataBuildDouble1(dTriMeshDataID g, - const void* Vertices, int VertexStride, int VertexCount, - const void* Indices, int IndexCount, int TriStride, - const void* Normals) -{ - dUASSERT(g, "argument not trimesh data"); - - g->Build(Vertices, VertexStride, VertexCount, - Indices, IndexCount, TriStride, - Normals, - false); -} - - -void dGeomTriMeshDataBuildDouble(dTriMeshDataID g, - const void* Vertices, int VertexStride, int VertexCount, - const void* Indices, int IndexCount, int TriStride) -{ - dGeomTriMeshDataBuildDouble1(g, Vertices, VertexStride, VertexCount, - Indices, IndexCount, TriStride, NULL); -} - - void dGeomTriMeshDataBuildSimple1(dTriMeshDataID g, const dReal* Vertices, int VertexCount, const dTriIndex* Indices, int IndexCount, const int* Normals) { -#ifdef dSINGLE - dGeomTriMeshDataBuildSingle1(g, - Vertices, 4 * sizeof(dReal), VertexCount, - Indices, IndexCount, 3 * sizeof(dTriIndex), - Normals); -#else - dGeomTriMeshDataBuildDouble1(g, Vertices, 4 * sizeof(dReal), VertexCount, - Indices, IndexCount, 3 * sizeof(dTriIndex), - Normals); -#endif + g->Build(Vertices, VertexCount, Indices, IndexCount); } - void dGeomTriMeshDataBuildSimple(dTriMeshDataID g, const dReal* Vertices, int VertexCount, const dTriIndex* Indices, int IndexCount) { - dGeomTriMeshDataBuildSimple1(g, - Vertices, VertexCount, Indices, IndexCount, - (const int*)NULL); + g->Build(Vertices, VertexCount, Indices, IndexCount); } +void dGeomTriMeshDataBuildDouble1(dTriMeshDataID g, + const void* Vertices, int VertexStride, int VertexCount, + const void* Indices, int IndexCount, int TriStride, + const void* Normals) +{ + //removed +} + +void dGeomTriMeshDataBuildDouble(dTriMeshDataID g, + const void* Vertices, int VertexStride, int VertexCount, + const void* Indices, int IndexCount, int TriStride) +{ + //removed +} + + void dGeomTriMeshDataPreprocess(dTriMeshDataID g) { dUASSERT(g, "argument not trimesh data"); @@ -561,8 +518,6 @@ dxTriMesh::~dxTriMesh(){ #endif // dTLS_ENABLED } - - void dxTriMesh::ClearTCCache() { /* dxTriMesh::ClearTCCache uses dArray's setSize(0) to clear the caches - @@ -649,29 +604,25 @@ bool dxTriMesh::controlGeometry_GetMergeSphereContacts(int &returnValue) void dxTriMesh::computeAABB() { - const dxTriMeshData* d = Data; - dVector3 c; const dMatrix3& R = final_posr->R; - const dVector3& pos = final_posr->pos; - dMultiply0_331( c, R, d->AABBCenter ); + dVector3 c; + dMultiply0_331( c, R, Data->AABBCenter ); + dAddVector3r4(c, final_posr->pos); - dReal xrange = dFabs(R[0] * Data->AABBExtents[0]) + - dFabs(R[1] * Data->AABBExtents[1]) + - dFabs(R[2] * Data->AABBExtents[2]); - dReal yrange = dFabs(R[4] * Data->AABBExtents[0]) + - dFabs(R[5] * Data->AABBExtents[1]) + - dFabs(R[6] * Data->AABBExtents[2]); - dReal zrange = dFabs(R[8] * Data->AABBExtents[0]) + - dFabs(R[9] * Data->AABBExtents[1]) + - dFabs(R[10] * Data->AABBExtents[2]); + dVector3& extents = *(dVector3*)Data->AABBExtents; - aabb[0] = c[0] + pos[0] - xrange; - aabb[1] = c[0] + pos[0] + xrange; - aabb[2] = c[1] + pos[1] - yrange; - aabb[3] = c[1] + pos[1] + yrange; - aabb[4] = c[2] + pos[2] - zrange; - aabb[5] = c[2] + pos[2] + zrange; + dReal a = dDotAbsVectors3r4(R, extents); + aabb[0] = c[0] - a; + aabb[1] = c[0] + a; + + a = dDotAbsVectors3r4(R + 4, extents); + aabb[2] = c[1] - a; + aabb[3] = c[1] + a; + + a = dDotAbsVectors3r4(R + 8, extents); + aabb[4] = c[2] - a; + aabb[5] = c[2] + a; } @@ -822,8 +773,9 @@ void dGeomTriMeshGetTriangle(dGeomID g, int Index, dVector3* v0, dVector3* v1, d dxTriMesh* Geom = (dxTriMesh*)g; - const dVector3& Position = *(const dVector3*)dGeomGetPosition(g); - const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(g); + dxPosR *dpr = g->GetRecomputePosR(); + const dVector3& Position = *(const dVector3*)dpr->pos; + const dMatrix3& Rotation = *(const dMatrix3*)dpr->R; dVector3 v[3]; FetchTriangle(Geom, Index, Position, Rotation, v); @@ -853,8 +805,9 @@ void dGeomTriMeshGetPoint(dGeomID g, int Index, dReal u, dReal v, dVector3 Out){ dxTriMesh* Geom = (dxTriMesh*)g; - const dVector3& Position = *(const dVector3*)dGeomGetPosition(g); - const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(g); + dxPosR *dpr = g->GetRecomputePosR(); + const dVector3& Position = *(const dVector3*)dpr->pos; + const dMatrix3& Rotation = *(const dMatrix3*)dpr->R; dVector3 dv[3]; FetchTriangle(Geom, Index, Position, Rotation, dv); diff --git a/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_plane.cpp b/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_plane.cpp index 7e262c00..b3342857 100644 --- a/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_plane.cpp +++ b/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_plane.cpp @@ -49,8 +49,9 @@ int dCollideTrimeshPlane( dxGeom *o1, dxGeom *o2, int flags, dContactGeom* conta const int contact_max = ( flags & NUMC_MASK ); // Cache trimesh position and rotation. - const dVector3& trimesh_pos = *(const dVector3*)dGeomGetPosition( trimesh ); - const dMatrix3& trimesh_R = *(const dMatrix3*)dGeomGetRotation( trimesh ); + dxPosR *posr = trimesh->GetRecomputePosR(); + const dVector3& trimesh_pos = *(const dVector3*)posr->pos; + const dMatrix3& trimesh_R = *(const dMatrix3*)posr->R; // // For all triangles. @@ -58,13 +59,10 @@ int dCollideTrimeshPlane( dxGeom *o1, dxGeom *o2, int flags, dContactGeom* conta VertexPointersEx VPE; VertexPointers &VP = VPE.vp; - ConversionArea VC; dReal alpha; dVector3 vertex; -#if !defined(dSINGLE) || 1 dVector3 int_vertex; // Intermediate vertex for double precision mode. -#endif // dSINGLE const unsigned uiTLSKind = trimesh->getParentSpaceTLSKind(); dIASSERT(uiTLSKind == plane->getParentSpaceTLSKind()); // The colliding spaces must use matching cleanup method @@ -82,7 +80,7 @@ int dCollideTrimeshPlane( dxGeom *o1, dxGeom *o2, int flags, dContactGeom* conta for ( int t = 0; t < tri_count; ++t ) { // Get triangle, which should also use callback. - bool ex_avail = trimesh->Data->Mesh.GetExTriangle( VPE, t, VC); + bool ex_avail = trimesh->Data->Mesh.GetExTriangle( VPE, t); // For each vertex. for ( int v = 0; v < 3; ++v ) @@ -101,12 +99,6 @@ int dCollideTrimeshPlane( dxGeom *o1, dxGeom *o2, int flags, dContactGeom* conta // Get Vertex // -#if defined(dSINGLE) && 0 // Always assign via intermediate array as otherwise it is an incapsulation violation - - dMultiply0_331( vertex, trimesh_R, (float*)( VP.Vertex[ v ] ) ); - -#else // dDOUBLE || 1 - // OPCODE data is in single precision format. int_vertex[ 0 ] = VP.Vertex[ v ]->x; int_vertex[ 1 ] = VP.Vertex[ v ]->y; @@ -114,8 +106,6 @@ int dCollideTrimeshPlane( dxGeom *o1, dxGeom *o2, int flags, dContactGeom* conta dMultiply0_331(vertex, trimesh_R, int_vertex ); -#endif // dSINGLE/dDOUBLE - dAddVector3r4(vertex, trimesh_pos); // diff --git a/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_ray.cpp b/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_ray.cpp index eb91e930..0d6ca654 100644 --- a/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_ray.cpp +++ b/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_ray.cpp @@ -39,8 +39,9 @@ int dCollideRTL(dxGeom* g1, dxGeom* RayGeom, int Flags, dContactGeom* Contacts, dxTriMesh* TriMesh = (dxTriMesh*)g1; - const dVector3& TLPosition = *(const dVector3*)dGeomGetPosition(TriMesh); - const dMatrix3& TLRotation = *(const dMatrix3*)dGeomGetRotation(TriMesh); + g1->recomputePosr(); + const dMatrix3& TLRotation = *(const dMatrix3*)g1->final_posr->R; + const dVector3& TLPosition = *(const dVector3*)g1->final_posr->pos; const unsigned uiTLSKind = TriMesh->getParentSpaceTLSKind(); dIASSERT(uiTLSKind == RayGeom->getParentSpaceTLSKind()); // The colliding spaces must use matching cleanup method diff --git a/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_sphere.cpp b/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_sphere.cpp index 199c0a75..9b71d794 100644 --- a/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_sphere.cpp +++ b/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_sphere.cpp @@ -288,8 +288,9 @@ int dCollideSTL(dxGeom* g1, dxGeom* SphereGeom, int Flags, dContactGeom* Contact dxTriMesh* TriMesh = (dxTriMesh*)g1; // Init - const dVector3& TLPosition = *(const dVector3*)dGeomGetPosition(TriMesh); - const dMatrix3& TLRotation = *(const dMatrix3*)dGeomGetRotation(TriMesh); + g1->recomputePosr(); + const dVector3& TLPosition = *(const dVector3*)g1->final_posr->pos; + const dMatrix3& TLRotation = *(const dMatrix3*)g1->final_posr->R; const unsigned uiTLSKind = TriMesh->getParentSpaceTLSKind(); dIASSERT(uiTLSKind == SphereGeom->getParentSpaceTLSKind()); // The colliding spaces must use matching cleanup method diff --git a/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_trimesh.cpp b/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_trimesh.cpp index e7b63e13..45ace7cd 100644 --- a/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_trimesh.cpp +++ b/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_trimesh.cpp @@ -625,6 +625,19 @@ dReal FindTriangleTriangleCollision(const dVector3 tri1[3], const dVector3 edges return maxdeep; } +int dCollideTri_Tri(dxGeom* g1, dxGeom* g2, int Flags, dContactGeom* Contacts, int Stride) +{ + return 0; +} +int dCollideTri_Mesh(dxGeom* g1, dxGeom* g2, int Flags, dContactGeom* Contacts, int Stride) +{ + return 0; +} +int dCollideMesh_Tri(dxGeom* g1, dxGeom* g2, int Flags, dContactGeom* Contacts, int Stride) +{ + return 0; +} + int dCollideTTL(dxGeom* g1, dxGeom* g2, int Flags, dContactGeom* Contacts, int Stride) { dIASSERT(Stride >= (int)sizeof(dContactGeom)); @@ -632,37 +645,39 @@ int dCollideTTL(dxGeom* g1, dxGeom* g2, int Flags, dContactGeom* Contacts, int S dIASSERT(g2->type == dTriMeshClass); dIASSERT((Flags & NUMC_MASK) >= 1); - dVector3 contactpoints[9]; - dVector4 normal; - dVector4 tri1plane; - dVector4 tri2plane; - dVector3 v1[3], v2[3]; - dVector3 v1e[3], v2e[3]; - dxTriMesh* TriMesh1 = (dxTriMesh*)g1; - dxTriMesh* TriMesh2 = (dxTriMesh*)g2; + dxTriMeshData *TriData1 = ((dxTriMesh*)g1)->Data; + dxTriMeshData *TriData2 = ((dxTriMesh*)g2)->Data; - const dVector3& TLPosition1 = *(const dVector3*)dGeomGetPosition(TriMesh1); - // TLRotation1 = column-major order - const dMatrix3& TLRotation1 = *(const dMatrix3*)dGeomGetRotation(TriMesh1); + if (TriData1->Mesh.GetNbTriangles() == 1) + { + if (TriData2->Mesh.GetNbTriangles() == 1) + return dCollideTri_Tri(g1, g2, Flags, Contacts, Stride); + return dCollideTri_Mesh(g1, g2, Flags, Contacts, Stride); + } + else if (TriData2->Mesh.GetNbTriangles() == 1) + return dCollideMesh_Tri(g1, g2, Flags, Contacts, Stride); + + + dxPosR *dpr1 = g1->GetRecomputePosR(); + const dVector3& TLPosition1 = *(const dVector3*)dpr1->pos; + const dMatrix3& TLRotation1 = *(const dMatrix3*)dpr1->R; + + dxPosR *dpr2 = g2->GetRecomputePosR(); + const dVector3& TLPosition2 = *(const dVector3*)dpr2->pos; + const dMatrix3& TLRotation2 = *(const dMatrix3*)dpr2->R; - const dVector3& TLPosition2 = *(const dVector3*)dGeomGetPosition(TriMesh2); - // TLRotation2 = column-major order - const dMatrix3& TLRotation2 = *(const dMatrix3*)dGeomGetRotation(TriMesh2); Matrix4x4 amatrix, bmatrix; - const unsigned uiTLSKind = TriMesh1->getParentSpaceTLSKind(); - dIASSERT(uiTLSKind == TriMesh2->getParentSpaceTLSKind()); // The colliding spaces must use matching cleanup method + const unsigned uiTLSKind = ((dxTriMesh*)g1)->getParentSpaceTLSKind(); + dIASSERT(uiTLSKind == ((dxTriMesh*)g2)->getParentSpaceTLSKind()); // The colliding spaces must use matching cleanup method TrimeshCollidersCache *pccColliderCache = GetTrimeshCollidersCache(uiTLSKind); AABBTreeCollider& Collider = pccColliderCache->_AABBTreeCollider; BVTCache &ColCache = pccColliderCache->ColCache; CONTACT_KEY_HASH_TABLE &hashcontactset = pccColliderCache->_hashcontactset; - ColCache.Model0 = &TriMesh1->Data->BVTree; - ColCache.Model1 = &TriMesh2->Data->BVTree; - - const bool contacts_unimportant = (Flags & CONTACTS_UNIMPORTANT) != 0; - const int max_contacts = (Flags & NUMC_MASK); + ColCache.Model0 = &TriData1->BVTree; + ColCache.Model1 = &TriData2->BVTree; ////Prepare contact list ClearContactSet(hashcontactset); @@ -687,6 +702,16 @@ int dCollideTTL(dxGeom* g1, dxGeom* g2, int Flags, dContactGeom* Contacts, int S int lastid2 = -11111; int OutContactsCount = 0; + const bool contacts_unimportant = (Flags & CONTACTS_UNIMPORTANT) != 0; + const int max_contacts = (Flags & NUMC_MASK); + + dVector3 contactpoints[9]; + dVector4 normal; + dVector4 tri1plane; + dVector4 tri2plane; + dVector3 v1[3], v2[3]; + dVector3 v1e[3], v2e[3]; + for (int i = 0; i < TriCount; i++) { int id1 = CollidingPairs[i].id0; diff --git a/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_util.cpp b/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_util.cpp index 6fe1da2f..bbad7815 100644 --- a/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_util.cpp +++ b/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_util.cpp @@ -515,8 +515,8 @@ int dBoxTouchesBox (const dVector3 p1, const dMatrix3 R1, int dClipEdgeToPlane( dVector3 &vEpnt0, dVector3 &vEpnt1, const dVector4& plPlane) { // calculate distance of edge points to plane - dReal fDistance0 = dCalcPointPlaneDistance( vEpnt0 ,plPlane ); - dReal fDistance1 = dCalcPointPlaneDistance( vEpnt1 ,plPlane ); + dReal fDistance0 = dCalcPointPlaneDistance( vEpnt0, plPlane ); + dReal fDistance1 = dCalcPointPlaneDistance( vEpnt1, plPlane ); // if both points are behind the plane if ( fDistance0 < 0 && fDistance1 < 0 ) @@ -532,20 +532,15 @@ int dClipEdgeToPlane( dVector3 &vEpnt0, dVector3 &vEpnt1, const dVector4& plPlan // if we have edge/plane intersection } else if ((fDistance0 > 0 && fDistance1 < 0) || ( fDistance0 < 0 && fDistance1 > 0)) { - // find intersection point of edge and plane - dVector3 vIntersectionPoint; - vIntersectionPoint[0]= vEpnt0[0]-(vEpnt0[0]-vEpnt1[0])*fDistance0/(fDistance0-fDistance1); - vIntersectionPoint[1]= vEpnt0[1]-(vEpnt0[1]-vEpnt1[1])*fDistance0/(fDistance0-fDistance1); - vIntersectionPoint[2]= vEpnt0[2]-(vEpnt0[2]-vEpnt1[2])*fDistance0/(fDistance0-fDistance1); - + dReal factor = fDistance0 / (fDistance1 - fDistance0); // clamp correct edge to intersection point if ( fDistance0 < 0 ) { - dCopyVector3r4(vEpnt0, vIntersectionPoint); + dCalcLerpVectors3r4(vEpnt0, vEpnt0, vEpnt1, factor); } else { - dCopyVector3r4(vEpnt1, vIntersectionPoint); + dCalcLerpVectors3r4(vEpnt1, vEpnt0, vEpnt1, factor); } return 1; } diff --git a/trunk/unmanaged/ubODE-OpenSim/ode/src/odemath.cpp b/trunk/unmanaged/ubODE-OpenSim/ode/src/odemath.cpp index ec25a41e..2ab91caf 100644 --- a/trunk/unmanaged/ubODE-OpenSim/ode/src/odemath.cpp +++ b/trunk/unmanaged/ubODE-OpenSim/ode/src/odemath.cpp @@ -77,6 +77,17 @@ int _dSafeNormalize3 (dVector3 a) } //#endif +int _dSafeNormalize3fast(dVector3 a) +{ + dReal l = dCalcVectorLengthSquare3(a); + if (l > dEpsilon) + { + dScaleVector3r4(a, dRecipSqrt(l)); + return 1; + } + return 0; +} + /* OLD VERSION */ /* void dNormalize3 (dVector3 a) diff --git a/trunk/unmanaged/ubODE-OpenSim/ode/src/odemath.h b/trunk/unmanaged/ubODE-OpenSim/ode/src/odemath.h index 52ea2240..75a37c29 100644 --- a/trunk/unmanaged/ubODE-OpenSim/ode/src/odemath.h +++ b/trunk/unmanaged/ubODE-OpenSim/ode/src/odemath.h @@ -27,7 +27,8 @@ #include "error.h" -int _dSafeNormalize3 (dVector3 a); +int _dSafeNormalize3(dVector3 a); +int _dSafeNormalize3fast(dVector3 a); int _dSafeNormalize4 (dVector4 a); ODE_PURE_INLINE void _dNormalize3(dVector3 a) @@ -44,6 +45,7 @@ ODE_PURE_INLINE void _dNormalize4(dVector4 a) // For internal use #define dSafeNormalize3(a) _dSafeNormalize3(a) +#define dSafeNormalize3fast(a) _dSafeNormalize3fast(a) #define dSafeNormalize4(a) _dSafeNormalize4(a) #define dNormalize3(a) _dNormalize3(a) #define dNormalize4(a) _dNormalize4(a) diff --git a/trunk/unmanaged/ubODE-OpenSim/ode/src/plane.cpp b/trunk/unmanaged/ubODE-OpenSim/ode/src/plane.cpp index b54e894c..f989bd54 100644 --- a/trunk/unmanaged/ubODE-OpenSim/ode/src/plane.cpp +++ b/trunk/unmanaged/ubODE-OpenSim/ode/src/plane.cpp @@ -48,20 +48,7 @@ dContactGeom::g1 and dContactGeom::g2. static void make_sure_plane_normal_has_unit_length (dxPlane *g) { - dReal l = g->p[0]*g->p[0] + g->p[1]*g->p[1] + g->p[2]*g->p[2]; - if (l > 0) { - l = dRecipSqrt(l); - g->p[0] *= l; - g->p[1] *= l; - g->p[2] *= l; - g->p[3] *= l; - } - else { - g->p[0] = 1; - g->p[1] = 0; - g->p[2] = 0; - g->p[3] = 0; - } + dSafeNormalize3(g->p); } @@ -72,13 +59,74 @@ dxGeom (space,0) p[0] = a; p[1] = b; p[2] = c; - p[3] = d; - make_sure_plane_normal_has_unit_length (this); + p[3] = dSafeNormalize3(p) ? d : 0; } - void dxPlane::computeAABB() { + // Planes that have normal vectors aligned along an axis can use a + // less comprehensive (half space) bounding box. + if (p[2] == 0) + { + if (p[1] == 0.0f) + { + // normal aligned with x-axis + if (p[0] > 0) + { + aabb[0] = -dInfinity; + aabb[1] = p[3]; + } + else + { + aabb[0] = -p[3]; + aabb[1] = dInfinity; + } + aabb[2] = -dInfinity; + aabb[3] = dInfinity; + aabb[4] = -dInfinity; + aabb[5] = dInfinity; + return; + } + if (p[0] == 0.0f) + { + // normal aligned with y-axis + aabb[0] = -dInfinity; + aabb[1] = dInfinity; + if(p[1] > 0) + { + aabb[2] = -dInfinity; + aabb[3] = p[3]; + } + else + { + aabb[2] = -p[3]; + aabb[3] = dInfinity; + } + aabb[4] = -dInfinity; + aabb[5] = dInfinity; + return; + } + } + if ( p[0] == 0.0f && p[1] == 0.0f ) + { + // normal aligned with z-axis + aabb[0] = -dInfinity; + aabb[1] = dInfinity; + aabb[2] = -dInfinity; + aabb[3] = dInfinity; + if (p[2] > 0) + { + aabb[4] = -dInfinity; + aabb[5] = p[3]; + } + else + { + aabb[4] = -p[3]; + aabb[5] = dInfinity; + } + return; + } + aabb[0] = -dInfinity; aabb[1] = dInfinity; aabb[2] = -dInfinity; @@ -86,61 +134,32 @@ void dxPlane::computeAABB() aabb[4] = -dInfinity; aabb[5] = dInfinity; - // Planes that have normal vectors aligned along an axis can use a - // less comprehensive (half space) bounding box. - - if ( p[1] == 0.0f && p[2] == 0.0f ) { - // normal aligned with x-axis - aabb[0] = (p[0] > 0) ? -dInfinity : -p[3]; - aabb[1] = (p[0] > 0) ? p[3] : dInfinity; - } else - if ( p[0] == 0.0f && p[2] == 0.0f ) { - // normal aligned with y-axis - aabb[2] = (p[1] > 0) ? -dInfinity : -p[3]; - aabb[3] = (p[1] > 0) ? p[3] : dInfinity; - } else - if ( p[0] == 0.0f && p[1] == 0.0f ) { - // normal aligned with z-axis - aabb[4] = (p[2] > 0) ? -dInfinity : -p[3]; - aabb[5] = (p[2] > 0) ? p[3] : dInfinity; - } } - -dGeomID dCreatePlane (dSpaceID space, - dReal a, dReal b, dReal c, dReal d) +dGeomID dCreatePlane (dSpaceID space, dReal a, dReal b, dReal c, dReal d) { - return new dxPlane (space,a,b,c,d); + return new dxPlane (space, a, b, c, d); } - void dGeomPlaneSetParams (dGeomID g, dReal a, dReal b, dReal c, dReal d) { dUASSERT (g && g->type == dPlaneClass,"argument not a plane"); - dxPlane *p = (dxPlane*) g; - p->p[0] = a; - p->p[1] = b; - p->p[2] = c; - p->p[3] = d; - make_sure_plane_normal_has_unit_length (p); + ((dxPlane*)g)->p[0] = a; + ((dxPlane*)g)->p[1] = b; + ((dxPlane*)g)->p[2] = c; + ((dxPlane*)g)->p[3] = dSafeNormalize3(((dxPlane*)g)->p) ? d : 0; dGeomMoved (g); } - void dGeomPlaneGetParams (dGeomID g, dVector4 result) { dUASSERT (g && g->type == dPlaneClass,"argument not a plane"); - dxPlane *p = (dxPlane*) g; - result[0] = p->p[0]; - result[1] = p->p[1]; - result[2] = p->p[2]; - result[3] = p->p[3]; + dCopyVector4(result, ((dxPlane*)g)->p); } - dReal dGeomPlanePointDepth (dGeomID g, dReal x, dReal y, dReal z) { dUASSERT (g && g->type == dPlaneClass,"argument not a plane"); - dxPlane *p = (dxPlane*) g; - return p->p[3] - p->p[0]*x - p->p[1]*y - p->p[2]*z; + dVector3& p = ((dxPlane*)g)->p; + return p[3] - p[0] * x - p[1] * y - p[2] * z; } diff --git a/trunk/unmanaged/ubODE-OpenSim/ode/src/quickstep.cpp b/trunk/unmanaged/ubODE-OpenSim/ode/src/quickstep.cpp index f690d3f0..f8b9cf63 100644 --- a/trunk/unmanaged/ubODE-OpenSim/ode/src/quickstep.cpp +++ b/trunk/unmanaged/ubODE-OpenSim/ode/src/quickstep.cpp @@ -2994,8 +2994,8 @@ void dxQuickStepIsland_Stage6c(dxQuickStepperStage6CallContext *stage6CallContex for (dxBody *const *bodycurr = body; bodycurr != bodyend; cforceMIDcurr+=6, bodycurr++) { dxBody *b = *bodycurr; - dSubtractVector3r4(b->lvel, cforceMIDcurr); - dSubtractVector3r4(b->avel, cforceMIDcurr + 3); + dSubtractVectors3r4(b->lvel, cforceMIDcurr); + dSubtractVectors3r4(b->avel, cforceMIDcurr + 3); } } } diff --git a/trunk/unmanaged/ubODE-OpenSim/ode/src/ray.cpp b/trunk/unmanaged/ubODE-OpenSim/ode/src/ray.cpp index ab36797f..6e4148b8 100644 --- a/trunk/unmanaged/ubODE-OpenSim/ode/src/ray.cpp +++ b/trunk/unmanaged/ubODE-OpenSim/ode/src/ray.cpp @@ -54,10 +54,10 @@ dxRay::dxRay (dSpaceID space, dReal _length) : dxGeom (space,1) void dxRay::computeAABB() { - dReal *pos = final_posr->pos; + const dReal *pos = final_posr->pos; dReal e = final_posr->R[2] * length; - if (e > 0) + if (e >= 0) { aabb[0] = pos[0]; aabb[1] = pos[0] + e; @@ -69,7 +69,7 @@ void dxRay::computeAABB() } e = final_posr->R[6] * length; - if (e > 0) + if (e >= 0) { aabb[2] = pos[1]; aabb[3] = pos[1] + e; @@ -81,7 +81,7 @@ void dxRay::computeAABB() } e = final_posr->R[10] * length; - if (e > 0) + if (e >= 0) { aabb[4] = pos[2]; aabb[5] = pos[2] + e; @@ -103,8 +103,7 @@ dGeomID dCreateRay (dSpaceID space, dReal length) void dGeomRaySetLength (dGeomID g, dReal length) { dUASSERT (g && g->type == dRayClass,"argument not a ray"); - dxRay *r = (dxRay*) g; - r->length = length; + ((dxRay*)g)->length = length; dGeomMoved (g); } @@ -112,8 +111,7 @@ void dGeomRaySetLength (dGeomID g, dReal length) dReal dGeomRayGetLength (dGeomID g) { dUASSERT (g && g->type == dRayClass,"argument not a ray"); - dxRay *r = (dxRay*) g; - return r->length; + return ((dxRay*)g)->length; } @@ -121,21 +119,21 @@ void dGeomRaySet (dGeomID g, dReal px, dReal py, dReal pz, dReal dx, dReal dy, dReal dz) { dUASSERT (g && g->type == dRayClass,"argument not a ray"); - g->recomputePosr(); - dReal* R = g->final_posr->R; - dReal* pos = g->final_posr->pos; - dVector3 n; - pos[0] = px; - pos[1] = py; - pos[2] = pz; + dVector3 n; n[0] = dx; n[1] = dy; n[2] = dz; dNormalize3(n); + dReal* R = g->final_posr->R; R[2] = n[0]; R[6] = n[1]; R[10] = n[2]; + + dReal* pos = g->final_posr->pos; + pos[0] = px; + pos[1] = py; + pos[2] = pz; dGeomMoved (g); } @@ -143,15 +141,13 @@ void dGeomRaySet (dGeomID g, dReal px, dReal py, dReal pz, void dGeomRayGet (dGeomID g, dVector3 start, dVector3 dir) { dUASSERT (g && g->type == dRayClass,"argument not a ray"); - g->recomputePosr(); - dReal* R = g->final_posr->R; - dReal* pos = g->final_posr->pos; - start[0] = pos[0]; - start[1] = pos[1]; - start[2] = pos[2]; + dxPosR *dpr = g->GetRecomputePosR(); + const dReal* R = dpr->R; dir[0] = R[2]; dir[1] = R[6]; dir[2] = R[10]; + const dReal* pos = dpr->pos; + dCopyVector3r4(start, pos); } void dGeomRaySetParams (dxGeom *g, int FirstContact, int BackfaceCull) @@ -224,19 +220,18 @@ int dGeomRayGetClosestHit (dxGeom *g) // if mode==1 then use the sphere exit contact, not the entry contact -static int ray_sphere_helper (dxRay *ray, dVector3 sphere_pos, dReal radius, - dContactGeom *contact, int mode) +static int ray_sphere_helper (const dxRay *ray, const dVector3 sphere_pos, const dReal radius, + dContactGeom *contact, const int mode) { dVector3 q; - dReal* pos = ray->final_posr->pos; - dReal* R = ray->final_posr->R; - q[0] = pos[0] - sphere_pos[0]; - q[1] = pos[1] - sphere_pos[1]; - q[2] = pos[2] - sphere_pos[2]; + const dReal* pos = ray->final_posr->pos; + const dReal* R = ray->final_posr->R; - dReal B = dCalcVectorDot3_14(q, R + 2); - dReal C = dCalcVectorLengthSquare3(q) - radius * radius; + dSubtractVectors3r4(q, pos, sphere_pos); + + const dReal B = dCalcVectorDot3_14(q, R + 2); + const dReal C = dCalcVectorLengthSquare3(q) - radius * radius; // note: if C <= 0 then the start of the ray is inside the sphere dReal k = B * B - C; if (k < 0) @@ -268,15 +263,11 @@ static int ray_sphere_helper (dxRay *ray, dVector3 sphere_pos, dReal radius, if(C < 0 || mode) { - contact->normal[0] = sphere_pos[0] - contact->pos[0]; - contact->normal[1] = sphere_pos[1] - contact->pos[1]; - contact->normal[2] = sphere_pos[2] - contact->pos[2]; + dSubtractVectors3r4(contact->normal, sphere_pos, contact->pos); } else { - contact->normal[0] = contact->pos[0] - sphere_pos[0]; - contact->normal[1] = contact->pos[1] - sphere_pos[1]; - contact->normal[2] = contact->pos[2] - sphere_pos[2]; + dSubtractVectors3r4(contact->normal, contact->pos, sphere_pos); } dNormalize3 (contact->normal); contact->depth = alpha; @@ -284,85 +275,122 @@ static int ray_sphere_helper (dxRay *ray, dVector3 sphere_pos, dReal radius, } -int dCollideRaySphere (dxGeom *o1, dxGeom *o2, int flags, - dContactGeom *contact, int skip) +int dCollideRaySphere (dxGeom *o1, dxGeom *o2, const int flags, + dContactGeom *contact, const int skip) { dIASSERT (skip >= (int)sizeof(dContactGeom)); dIASSERT (o1->type == dRayClass); dIASSERT (o2->type == dSphereClass); dIASSERT ((flags & NUMC_MASK) >= 1); - dxRay *ray = (dxRay*) o1; - dxSphere *sphere = (dxSphere*) o2; - contact->g1 = ray; - contact->g2 = sphere; + dxPosR *dpr = o1->final_posr; + const dReal* pos = dpr->pos; + const dReal* R = dpr->R; + + dpr = o2->final_posr; + const dReal* sphere_pos = dpr->pos; + + dVector3 q; + dSubtractVectors3r4(q, pos, sphere_pos); + + const dReal radius = ((dxSphere*)o2)->radius; + const dReal C = dCalcVectorLengthSquare3(q) - radius * radius; + + //if C <= 0 then the start of the ray is inside the sphere + if (C <= 0 && (o1->gflags & RAY_BACKFACECULL) != 0) + return 0; + + const dReal B = dCalcVectorDot3_14(q, R + 2); + dReal k = B * B - C; + if (k < 0) + return 0; + k = dSqrt(k); + dReal alpha = -B - k; + if (alpha < 0) + { + alpha = -B + k; + if (alpha < 0) + return 0; + } + if (alpha > ((dxRay*)o1)->length) + return 0; + + contact->pos[0] = pos[0] + alpha * R[2]; + contact->pos[1] = pos[1] + alpha * R[6]; + contact->pos[2] = pos[2] + alpha * R[10]; + + if (C < 0) + dSubtractVectors3r4(contact->normal, sphere_pos, contact->pos); + else + dSubtractVectors3r4(contact->normal, contact->pos, sphere_pos); + + dNormalize3(contact->normal); + contact->depth = alpha; + contact->g1 = o1; + contact->g2 = o2; contact->side1 = -1; contact->side2 = -1; - return ray_sphere_helper (ray,sphere->final_posr->pos,sphere->radius,contact,0); + + return 1; } -int dCollideRayBox (dxGeom *o1, dxGeom *o2, int flags, - dContactGeom *contact, int skip) +int dCollideRayBox (dxGeom *o1, dxGeom *o2, const int flags, + dContactGeom *contact, const int skip) { dIASSERT (skip >= (int)sizeof(dContactGeom)); dIASSERT (o1->type == dRayClass); dIASSERT (o2->type == dBoxClass); dIASSERT ((flags & NUMC_MASK) >= 1); - dxRay *ray = (dxRay*) o1; - dxBox *box = (dxBox*) o2; + dxPosR *dpr = o1->final_posr; + const dReal* pos = dpr->pos; + const dReal* R = dpr->R; - contact->g1 = ray; - contact->g2 = box; - contact->side1 = -1; - contact->side2 = -1; + dpr = o2->final_posr; + const dReal* boxpos = dpr->pos; + const dReal* boxR = dpr->R; - int i; + dVector3 tmp; + tmp[0] = R[2]; + tmp[1] = R[6]; + tmp[2] = R[10]; - dReal* pos = ray->final_posr->pos; - dReal* R = ray->final_posr->R; - - dReal* boxpos = box->final_posr->pos; - dReal* boxR = box->final_posr->R; + dVector3 v; + dMultiply1_331(v, boxR, tmp); + if (v[0] == 0 && v[1] == 0 && v[2] == 0) + return 0; // compute the start and delta of the ray relative to the box. // we will do all subsequent computations in this box-relative coordinate // system. we have to do a translation and rotation for each point. - dVector3 tmp, s ,v; - tmp[0] = pos[0] - boxpos[0]; - tmp[1] = pos[1] - boxpos[1]; - tmp[2] = pos[2] - boxpos[2]; - dMultiply1_331 (s, boxR, tmp); - tmp[0] = R[2]; - tmp[1] = R[6]; - tmp[2] = R[10]; - dMultiply1_331 (v, boxR, tmp); + dVector3 start; + dSubtractVectors3r4(tmp, pos, boxpos); + dMultiply1_331 (start, boxR, tmp); // mirror the line so that v has all components >= 0 dVector3 sign; - for (i=0; i<3; i++) + int i; + for (i = 0; i < 3; ++i) { if (v[i] < 0) { - s[i] = -s[i]; + start[i] = -start[i]; v[i] = -v[i]; sign[i] = 1; } else sign[i] = -1; } - // compute the half-sides of the box - dReal h[3]; - h[0] = box->halfside[0]; - h[1] = box->halfside[1]; - h[2] = box->halfside[2]; + const dReal *h = ((dxBox*)o2)->halfside; // do a few early exit tests - if ((s[0] < -h[0] && v[0] <= 0) || s[0] > h[0] || - (s[1] < -h[1] && v[1] <= 0) || s[1] > h[1] || - (s[2] < -h[2] && v[2] <= 0) || s[2] > h[2] || - (v[0] == 0 && v[1] == 0 && v[2] == 0)) { + if (start[0] > h[0] || start[1] > h[1] || start[2] > h[2]) + return 0; + + if ((o1->gflags & RAY_BACKFACECULL) != 0) + { + if (start[0] > -h[0] && start[1] > -h[1] && start[2] > -h[2]) return 0; } @@ -370,17 +398,18 @@ int dCollideRayBox (dxGeom *o1, dxGeom *o2, int flags, dReal lo = -dInfinity; dReal hi = dInfinity; int nlo = 0, nhi = 0; - for (i=0; i<3; i++) + for (i = 0; i < 3; i++) { if (v[i] != 0) { - dReal k = (-h[i] - s[i])/v[i]; + dReal invV = REAL(1.0) / v[i]; + dReal k = -(h[i] + start[i]) * invV; if (k > lo) { lo = k; nlo = i; } - k = (h[i] - s[i])/v[i]; + k = (h[i] - start[i]) * invV; if (k < hi) { hi = k; @@ -405,9 +434,10 @@ int dCollideRayBox (dxGeom *o1, dxGeom *o2, int flags, n = nhi; } - if (alpha < 0 || alpha > ray->length) + if (alpha < 0 || alpha > ((dxRay*)o1)->length) return 0; + contact->pos[0] = pos[0] + alpha * R[2]; contact->pos[1] = pos[1] + alpha * R[6]; contact->pos[2] = pos[2] + alpha * R[10]; @@ -426,47 +456,46 @@ int dCollideRayBox (dxGeom *o1, dxGeom *o2, int flags, } contact->depth = alpha; + contact->g1 = o1; + contact->g2 = o2; + contact->side1 = -1; + contact->side2 = -1; return 1; } -int dCollideRayCapsule (dxGeom *o1, dxGeom *o2, - int flags, dContactGeom *contact, int skip) +int dCollideRayCapsule (dxGeom *o1, dxGeom *o2, const int flags, dContactGeom *contact, const int skip) { dIASSERT (skip >= (int)sizeof(dContactGeom)); dIASSERT (o1->type == dRayClass); dIASSERT (o2->type == dCapsuleClass); dIASSERT ((flags & NUMC_MASK) >= 1); - dxRay *ray = (dxRay*) o1; - dxCapsule *ccyl = (dxCapsule*) o2; - - contact->g1 = ray; - contact->g2 = ccyl; - contact->side1 = -1; - contact->side2 = -1; + const dxRay *ray = (dxRay*) o1; + const dxCapsule *ccyl = (dxCapsule*) o2; const dReal radiusSQ = ccyl->radius * ccyl->radius; const dReal lz2 = ccyl->halfLenZ; - dReal* pos = ray->final_posr->pos; - dReal* R = ray->final_posr->R; - dReal* cpos = ccyl->final_posr->pos; - dReal* cR = ccyl->final_posr->R; + dxPosR* dpr = ray->final_posr; + const dReal* pos = dpr->pos; + const dReal* R = dpr->R; + dpr = ccyl->final_posr; + const dReal* cpos = dpr->pos; + const dReal* cR = dpr->R; // compute some useful info - dVector3 cs, q, r; - dReal C,k; - cs[0] = pos[0] - cpos[0]; - cs[1] = pos[1] - cpos[1]; - cs[2] = pos[2] - cpos[2]; - k = dCalcVectorDot3_41(cR + 2, cs); // position of ray start along ccyl axis + dVector3 cs; + dSubtractVectors3r4(cs, pos, cpos); + + dReal k = dCalcVectorDot3_41(cR + 2, cs); // position of ray start along ccyl axis + dVector3 q; q[0] = k * cR[2] - cs[0]; q[1] = k * cR[6] - cs[1]; q[2] = k * cR[10] - cs[2]; - C = dCalcVectorLengthSquare3(q) - radiusSQ; - // if C < 0 then ray start position within infinite extension of cylinder + dReal C = dCalcVectorLengthSquare3(q) - radiusSQ; + // if C < 0 then ray start position within infinite extension of cylinder // see if ray start position is inside the capped cylinder int inside_ccyl = 0; if (C < 0) @@ -483,6 +512,9 @@ int dCollideRayCapsule (dxGeom *o1, dxGeom *o2, inside_ccyl = 1; } + if(inside_ccyl && ((o1->gflags & RAY_BACKFACECULL) != 0)) + return 0; + // compute ray collision with infinite cylinder, except for the case where // the ray is outside the capped cylinder but within the infinite cylinder // (it that case the ray can only hit endcaps) @@ -497,6 +529,7 @@ int dCollideRayCapsule (dxGeom *o1, dxGeom *o2, else { dReal uv = dCalcVectorDot3_44(cR + 2, R + 2); + dVector3 r; r[0] = uv * cR[2] - R[2]; r[1] = uv * cR[6] - R[6]; r[2] = uv * cR[10] - R[10]; @@ -545,9 +578,7 @@ int dCollideRayCapsule (dxGeom *o1, dxGeom *o2, contact->pos[0] = pos[0] + alpha * R[2]; contact->pos[1] = pos[1] + alpha * R[6]; contact->pos[2] = pos[2] + alpha * R[10]; - q[0] = contact->pos[0] - cpos[0]; - q[1] = contact->pos[1] - cpos[1]; - q[2] = contact->pos[2] - cpos[2]; + dSubtractVectors3r4(q, contact->pos, cpos); k = dCalcVectorDot3_14(q, cR + 2); if (k >= -lz2 && k <= lz2) { @@ -566,6 +597,10 @@ int dCollideRayCapsule (dxGeom *o1, dxGeom *o2, dNormalize3 (contact->normal); contact->depth = alpha; + contact->g1 = o1; + contact->g2 = o2; + contact->side1 = -1; + contact->side2 = -1; return 1; } @@ -584,7 +619,17 @@ int dCollideRayCapsule (dxGeom *o1, dxGeom *o2, q[0] = cpos[0] + k * cR[2]; q[1] = cpos[1] + k * cR[6]; q[2] = cpos[2] + k * cR[10]; - return ray_sphere_helper (ray, q, ccyl->radius, contact, inside_ccyl); + + int ret = ray_sphere_helper(ray, q, ccyl->radius, contact, inside_ccyl); + if (ret > 0) + { + contact->g1 = o1; + contact->g2 = o2; + contact->side1 = -1; + contact->side2 = -1; + return ret; + } + return 0; } @@ -596,19 +641,28 @@ int dCollideRayPlane (dxGeom *o1, dxGeom *o2, int flags, dIASSERT (o2->type == dPlaneClass); dIASSERT ((flags & NUMC_MASK) >= 1); - dxRay *ray = (dxRay*) o1; - dxPlane *plane = (dxPlane*) o2; + const dxRay *ray = (dxRay*) o1; + const dxPlane *plane = (dxPlane*) o2; - dReal* pos = ray->final_posr->pos; - dReal* R = ray->final_posr->R; + const dReal* pos = ray->final_posr->pos; + const dReal* R = ray->final_posr->R; - dReal alpha = plane->p[3] - dCalcVectorDot3 (plane->p, pos); - // note: if alpha > 0 the starting point is below the plane - dReal nsign = (alpha > 0) ? REAL(-1.0) : REAL(1.0); - dReal k = dCalcVectorDot3_14(plane->p, R + 2); + const dReal k = dCalcVectorDot3_14(plane->p, R + 2); if (k==0) return 0; // ray parallel to plane + // note: if alpha > 0 the starting point is below the plane + dReal alpha = plane->p[3] - dCalcVectorDot3(plane->p, pos); + int nsign; + if (alpha > 0) + { + if (k > 0 && ((o1->gflags & RAY_BACKFACECULL) != 0)) + return 0; + nsign = -1; + } + else + nsign = 1; + alpha /= k; if (alpha < 0 || alpha > ray->length) return 0; @@ -617,21 +671,13 @@ int dCollideRayPlane (dxGeom *o1, dxGeom *o2, int flags, contact->pos[1] = pos[1] + alpha * R[6]; contact->pos[2] = pos[2] + alpha * R[10]; if(nsign > 0) - { - contact->normal[0] = plane->p[0]; - contact->normal[1] = plane->p[1]; - contact->normal[2] = plane->p[2]; - } + dCopyVector3r4(contact->normal, plane->p); else - { - contact->normal[0] = -plane->p[0]; - contact->normal[1] = -plane->p[1]; - contact->normal[2] = -plane->p[2]; - } + dCopyNegatedVector3r4(contact->normal, plane->p); contact->depth = alpha; - contact->g1 = ray; - contact->g2 = plane; + contact->g1 = o1; + contact->g2 = o2; contact->side1 = -1; contact->side2 = -1; return 1; diff --git a/trunk/unmanaged/ubODE-OpenSim/ode/src/sphere.cpp b/trunk/unmanaged/ubODE-OpenSim/ode/src/sphere.cpp index 24c075f6..14e11950 100644 --- a/trunk/unmanaged/ubODE-OpenSim/ode/src/sphere.cpp +++ b/trunk/unmanaged/ubODE-OpenSim/ode/src/sphere.cpp @@ -197,7 +197,7 @@ int dCollideSphereBox (dxGeom *o1, dxGeom *o2, int flags, { // sphere center inside box. find closest face to `t' dFabsVector3r4(q, t); - dSubtractVector3r4(l, q); + dSubtractVectors3r4(l, q); dReal min_distance = l[0]; int mini = 0;