admin/opensim-libs
1
0
Fork 0
mirror of git://opensimulator.org/git/opensim-libs synced 2026-08-14 00:58:00 +00:00
Code Issues Packages Projects Releases Wiki Activity

several changes on ubode. remove more ode features not in use and with some performance cost

Browse Source
This commit is contained in:
UbitUmarov
2022-07-13 18:35:09 +01:00
parent e8ca920ab0
commit 1c8165135f
51 changed files with 1539 additions and 4270 deletions
Download Patch File Download Diff File Expand all files Collapse all files
trunk/unmanaged/ubODE-OpenSim/OPCODE/Ice/IceFPU.h
+49 -6
View File
@@ -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);
trunk/unmanaged/ubODE-OpenSim/OPCODE/Ice/IcePoint.h
+42 -106
View File
@@ -14,6 +14,14 @@
#if defined(__AVX__)
#include <immintrin.h>
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;
trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_AABBCollider.cpp
+8 -87
View File
@@ -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())
trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_BaseModel.cpp
+2 -24
View File
@@ -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;
trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_BaseModel.h
+1 -24
View File
@@ -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__
trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_BoxBoxOverlap.h
+119 -34
View File
@@ -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
}
trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_Common.h
+24 -6
View File
@@ -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)<GetMax(1)) return FALSE;
if(box.GetMax(2)<GetMax(2)) return FALSE;
return TRUE;
}
#endif
}
Point mCenter; //!< Box center
Point mExtents; //!< Box extents
trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_HybridModel.cpp
+9 -25
View File
@@ -224,6 +224,7 @@ bool HybridModel::Build(const OPCODECREATE& create)
mNbLeaves = Data.mNbLeaves; // Keep track of it
// Special case for 1-leaf meshes
if(mNbLeaves==1)
{
mModelCode |= OPC_SINGLE_NODE;
@@ -277,7 +278,7 @@ bool HybridModel::Build(const OPCODECREATE& create)
}
// 3) Create an optimized tree according to user-settings
if(!CreateTree(create.mNoLeaf, create.mQuantized)) goto FreeAndExit;
if(!CreateTree()) goto FreeAndExit;
// 3-2) Create optimized tree
if(!mTree->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;
*/
}
trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_LSSCollider.cpp
+8 -88
View File
@@ -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())
trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_MeshInterface.cpp
+2 -179
View File
@@ -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;i<mNbTris;i++)
{
GetTriangle(VP, i, VC);
GetTriangle(VP, i);
if( (VP.Vertex[0]==VP.Vertex[1])
|| (VP.Vertex[1]==VP.Vertex[2])
@@ -204,41 +163,6 @@ udword MeshInterface::CheckTopology() const
return NbDegenerate;
}
#ifdef OPC_USE_CALLBACKS
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Callback control: setups object callback. Must provide triangle-vertices for a given triangle index.
* \param callback [in] user-defined callback
* \param user_data [in] user-defined data
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool MeshInterface::SetCallback(RequestCallback callback, void* user_data)
{
if(!callback) return SetIceError("MeshInterface::SetCallback: callback pointer is null");
mObjCallback = callback;
mUserData = user_data;
return true;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Callback control: setups object ex-callback. Must provide triangle-vertices and vertex indice for a given triangle index.
* \param callback [in] user-defined callback
* \param user_data [in] user-defined data
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool MeshInterface::SetExCallback(RequestExCallback callback, void* user_data)
{
// if(!callback) -- allow nulls
mObjExCallback = callback;
mExUserData = user_data;
return true;
}
#else
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Pointers control: setups object pointers. Must provide access to faces and vertices for a given object.
@@ -255,98 +179,6 @@ bool MeshInterface::SetPointers(const IndexedTriangle* tris, const Point* verts)
mVerts = verts;
return true;
}
#ifdef OPC_USE_STRIDE
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Strides control
* \param tri_stride [in] size of a triangle in bytes. The first sizeof(IndexedTriangle) bytes are used to get vertex indices.
* \param vertex_stride [in] size of a vertex in bytes. The first sizeof(Point) bytes are used to get vertex position.
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool MeshInterface::SetStrides(udword tri_stride, udword vertex_stride)
{
if(tri_stride<sizeof(IndexedTriangle)) return SetIceError("MeshInterface::SetStrides: invalid triangle stride", null);
if(vertex_stride<sizeof(Point)) return SetIceError("MeshInterface::SetStrides: invalid vertex stride", null);
mTriStride = tri_stride;
mVertexStride = vertex_stride;
return true;
}
#endif
#endif
#ifndef OPC_USE_CALLBACKS
#ifdef OPC_USE_STRIDE
void MeshInterface::FetchTriangleFromSingles(VertexPointers& vp, udword index, ConversionArea vc) const
{
const IndexedTriangle* T = (const IndexedTriangle*)(((ubyte*)mTris) + index * mTriStride);
const Point* Verts = GetVerts();
udword VertexStride = GetVertexStride();
vp.Vertex[0] = (const Point*)(((ubyte*)Verts) + T->mVRef[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;i<mNbTris;i++)
{
@@ -388,6 +212,5 @@ bool MeshInterface::RemapClient(udword nb_indices, const dTriIndex* permutation)
}
DELETEARRAY(Tmp);
#endif
return true;
}
trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_MeshInterface.h
+8 -143
View File
@@ -44,30 +44,6 @@
dTriIndex Index[3];
};
typedef Point ConversionArea[3];
#ifdef OPC_USE_CALLBACKS
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* User-callback, called by OPCODE to request vertices from the app.
* \param triangle_index [in] face index for which the system is requesting the vertices
* \param triangle [out] triangle's vertices (must be provided by the user)
* \param user_data [in] user-defined data from SetCallback()
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
typedef void (*RequestCallback) (udword triangle_index, VertexPointers& triangle, void* user_data);
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* User-callback, called by OPCODE to request vertex indices from the app.
* \param triangle_index [in] face index for which the system is requesting the vertices
* \param triangle [out] triangle's vertices with indices (must be provided by the user)
* \param user_data [in] user-defined data from SetExCallback()
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
typedef void (*RequestExCallback) (udword triangle_index, VertexPointersEx& triangle, void* user_data);
#endif
class OPCODE_API MeshInterface
{
public:
@@ -80,33 +56,6 @@
inline_ void SetNbTriangles(udword nb) { mNbTris = nb; }
inline_ void SetNbVertices(udword nb) { mNbVerts = nb; }
#ifdef OPC_USE_CALLBACKS
// Callback settings
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Callback control: setups object callback. Must provide triangle-vertices for a given triangle index.
* \param callback [in] user-defined callback
* \param user_data [in] user-defined data
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool SetCallback(RequestCallback callback, void* user_data);
inline_ void* GetUserData() const { return mUserData; }
inline_ RequestCallback GetCallback() const { return mObjCallback; }
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Callback control: setups object callback. Must provide triangle-vertices for a given triangle index.
* \param callback [in] user-defined callback
* \param user_data [in] user-defined data
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool SetExCallback(RequestExCallback callback, void* user_data);
inline_ void* GetExUserData() const { return mExUserData; }
inline_ RequestExCallback GetExCallback() const { return mObjExCallback; }
#else
// Pointers settings
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -121,40 +70,10 @@
inline_ const IndexedTriangle* GetTris() const { return mTris; }
inline_ const Point* GetVerts() const { return mVerts; }
#ifdef OPC_USE_STRIDE
// Strides settings
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Strides control
* \param tri_stride [in] size of a triangle in bytes. The first sizeof(IndexedTriangle) bytes are used to get vertex indices.
* \param vertex_stride [in] size of a vertex in bytes. The first sizeof(Point) bytes are used to get vertex position.
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool SetStrides(udword tri_stride=sizeof(IndexedTriangle), udword vertex_stride=sizeof(Point));
inline_ udword GetTriStride() const { return mTriStride; }
inline_ udword GetVertexStride() const { return mVertexStride; }
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Single/Double control
* \param value [in] Indicates if mesh data is provided as array of \c single values. If \c false, data is expected to contain \c double elements.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ void SetSingle(bool value)
{
mFetchTriangle = (value ? &MeshInterface::FetchTriangleFromSingles : &MeshInterface::FetchTriangleFromDoubles);
mFetchExTriangle = (value ? &MeshInterface::FetchExTriangleFromSingles : &MeshInterface::FetchExTriangleFromDoubles);
}
#else
inline_ bool SetStrides(udword tri_stride=sizeof(IndexedTriangle), udword vertex_stride=sizeof(Point)) { return true; }
inline_ void SetSingle(bool value) {}
inline_ udword GetTriStride() const { return sizeof(IndexedTriangle); }
inline_ udword GetVertexStride() const { return sizeof(Point); }
#endif
#endif
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
@@ -164,67 +83,29 @@
* \param vc [in,out] storage required for data conversion (pass local variable with same scope as \a vp, as \a vp may point to this memory on return)
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ void GetTriangle(VertexPointers& vp, udword index, ConversionArea vc) const
inline_ void GetTriangle(VertexPointers& vp, udword index) const
{
#ifdef OPC_USE_CALLBACKS
(mObjCallback)(index, vp, mUserData);
#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).*mFetchTriangle)(vp, index, vc);
#else
const Point* Verts = GetVerts();
const IndexedTriangle* T = &mTris[index];
vp.Vertex[0] = &Verts[T->mVRef[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__
trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_Model.cpp
+2 -33
View File
@@ -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;
}
trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_Model.h
-13
View File
@@ -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();
};
trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_OBBCollider.cpp
+1 -80
View File
@@ -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())
trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_OptimizedTree.cpp
+39 -395
View File
@@ -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;i<mNbNodes;i++) \
{ \
if(fabsf(Nodes[i].mAABB.mCenter.x)>CMax.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<<nbc)-1)/CMax.x : 0.0f; \
CQuantCoeff.y = CMax.y!=0.0f ? float((1<<nbc)-1)/CMax.y : 0.0f; \
CQuantCoeff.z = CMax.z!=0.0f ? float((1<<nbc)-1)/CMax.z : 0.0f; \
EQuantCoeff.x = EMax.x!=0.0f ? float((1<<nbe)-1)/EMax.x : 0.0f; \
EQuantCoeff.y = EMax.y!=0.0f ? float((1<<nbe)-1)/EMax.y : 0.0f; \
EQuantCoeff.z = EMax.z!=0.0f ? float((1<<nbe)-1)/EMax.z : 0.0f; \
/* Compute and save dequantization coeffs */ \
mCenterCoeff.x = CQuantCoeff.x!=0.0f ? 1.0f / CQuantCoeff.x : 0.0f; \
mCenterCoeff.y = CQuantCoeff.y!=0.0f ? 1.0f / CQuantCoeff.y : 0.0f; \
mCenterCoeff.z = CQuantCoeff.z!=0.0f ? 1.0f / CQuantCoeff.z : 0.0f; \
mExtentsCoeff.x = EQuantCoeff.x!=0.0f ? 1.0f / EQuantCoeff.x : 0.0f; \
mExtentsCoeff.y = EQuantCoeff.y!=0.0f ? 1.0f / EQuantCoeff.y : 0.0f; \
mExtentsCoeff.z = EQuantCoeff.z!=0.0f ? 1.0f / EQuantCoeff.z : 0.0f; \
#define PERFORM_QUANTIZATION \
/* Quantize */ \
mNodes[i].mAABB.mCenter[0] = sword(Nodes[i].mAABB.mCenter.x * CQuantCoeff.x); \
mNodes[i].mAABB.mCenter[1] = sword(Nodes[i].mAABB.mCenter.y * CQuantCoeff.y); \
mNodes[i].mAABB.mCenter[2] = sword(Nodes[i].mAABB.mCenter.z * CQuantCoeff.z); \
mNodes[i].mAABB.mExtents[0] = uword(Nodes[i].mAABB.mExtents.x * EQuantCoeff.x); \
mNodes[i].mAABB.mExtents[1] = uword(Nodes[i].mAABB.mExtents.y * EQuantCoeff.y); \
mNodes[i].mAABB.mExtents[2] = uword(Nodes[i].mAABB.mExtents.z * EQuantCoeff.z); \
/* Fix quantized boxes */ \
if(gFixQuantized) \
{ \
/* Make sure the quantized box is still valid */ \
Point Max = Nodes[i].mAABB.mCenter + Nodes[i].mAABB.mExtents; \
Point Min = Nodes[i].mAABB.mCenter - Nodes[i].mAABB.mExtents; \
/* For each axis */ \
for(udword j=0;j<3;j++) \
{ /* Dequantize the box center */ \
float qc = float(mNodes[i].mAABB.mCenter[j]) * mCenterCoeff[j]; \
bool FixMe=true; \
do \
{ /* Dequantize the box extent */ \
float qe = float(mNodes[i].mAABB.mExtents[j]) * mExtentsCoeff[j]; \
/* Compare real & dequantized values */ \
if(qc+qe<Max[j] || qc-qe>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;i<mNbNodes;i++)
{
PERFORM_QUANTIZATION
REMAP_DATA(mData)
}
DELETEARRAY(Nodes);
}
return true;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Refits the collision tree after vertices have been modified.
* \param mesh_interface [in] mesh interface for current model
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool AABBQuantizedTree::Refit(const MeshInterface* /*mesh_interface*/)
{
ASSERT(!"Not implemented since requantizing is painful !");
return false;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Walks the tree and call the user back for each node.
* \param callback [in] walking callback
* \param user_data [in] callback's user data
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool AABBQuantizedTree::Walk(GenericWalkingCallback callback, void* user_data) const
{
if(!callback) return false;
struct Local
{
static void _Walk(const AABBQuantizedNode* current_node, GenericWalkingCallback callback, void* user_data)
{
if(!current_node || !(callback)(current_node, user_data)) return;
if(!current_node->IsLeaf())
{
_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;i<mNbNodes;i++)
{
PERFORM_QUANTIZATION
REMAP_DATA(mPosData)
REMAP_DATA(mNegData)
}
DELETEARRAY(Nodes);
}
return true;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Refits the collision tree after vertices have been modified.
* \param mesh_interface [in] mesh interface for current model
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool AABBQuantizedNoLeafTree::Refit(const MeshInterface* /*mesh_interface*/)
{
ASSERT(!"Not implemented since requantizing is painful !");
return false;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Walks the tree and call the user back for each node.
* \param callback [in] walking callback
* \param user_data [in] callback's user data
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool AABBQuantizedNoLeafTree::Walk(GenericWalkingCallback callback, void* user_data) const
{
if(!callback) return false;
struct Local
{
static void _Walk(const AABBQuantizedNoLeafNode* current_node, GenericWalkingCallback callback, void* user_data)
{
if(!current_node || !(callback)(current_node, user_data)) return;
if(!current_node->HasPosLeaf()) _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;
}
trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_OptimizedTree.h
-18
View File
@@ -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__
trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_PlanesCollider.cpp
+1 -80
View File
@@ -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<<nb_planes)-1;
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(), 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<<nb_planes)-1;
// 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(), 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())
trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_PlanesCollider.h
-1
View File
@@ -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);
trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_RayAABBOverlap.h
+18 -14
View File
@@ -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;
}
trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_RayCollider.cpp
+2 -43
View File
@@ -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
trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_Settings.h
-9
View File
@@ -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
trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_SphereCollider.cpp
+1 -80
View File
@@ -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())
trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_TreeBuilders.cpp
+4 -8
View File
@@ -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;i<nb_prims;i++)
{
// Get current triangle-vertices
mIMesh->GetTriangle(VP, primitives[i], VC);
mIMesh->GetTriangle(VP, primitives[i]);
// Update split value
SplitValue += (*VP.Vertex[0])[axis];
SplitValue += (*VP.Vertex[1])[axis];
trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_TreeCollider.cpp
+8 -403
View File
@@ -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());
}
}
trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_TreeCollider.h
-27
View File
@@ -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);
trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_TriBoxOverlap.h
+58 -88
View File
@@ -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
trunk/unmanaged/ubODE-OpenSim/OPENSIM-README.txt
+1 -1
View File
@@ -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
trunk/unmanaged/ubODE-OpenSim/include/ode/collision.h
+1 -3
View File
@@ -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.
*
trunk/unmanaged/ubODE-OpenSim/include/ode/common.h
-2
View File
@@ -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.
trunk/unmanaged/ubODE-OpenSim/include/ode/odemath.h
+200 -34
View File
@@ -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 <ode/odemath_legacy.h>
trunk/unmanaged/ubODE-OpenSim/include/ode/version.h
+1 -1
View File
@@ -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
trunk/unmanaged/ubODE-OpenSim/ode/src/box.cpp
+12 -10
View File
@@ -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; i<num; i++)
{
@@ -1034,7 +1036,7 @@ done:
{
dContactGeom *target = CONTACT(contact, 3 * skip);
dAddVectors3r4(target->pos, CONTACT(contact, skip)->pos, CONTACT(contact, 2 * skip)->pos);
dSubtractVector3r4(target->pos, p);
dSubtractVectors3r4(target->pos, p);
target->depth = d4;
ret++;
}
trunk/unmanaged/ubODE-OpenSim/ode/src/capsule.cpp
+62 -61
View File
@@ -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);
trunk/unmanaged/ubODE-OpenSim/ode/src/collision_kernel.cpp
+6 -7
View File
@@ -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);
}
trunk/unmanaged/ubODE-OpenSim/ode/src/collision_kernel.h
+12 -1
View File
@@ -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);
trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_box.cpp
+10 -19
View File
@@ -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;
trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_ccylinder.cpp → trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_capsule.cpp
+343 -313
View File
@@ -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;
}
}
}
trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_distance.cpp
-1277
View File
File diff suppressed because it is too large Load Diff
trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_internal.h
+16 -101
View File
@@ -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<class T> 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_
trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_opcode.cpp
+145 -192
View File
@@ -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);
trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_plane.cpp
+4 -14
View File
@@ -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);
//
trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_ray.cpp
+3 -2
View File
@@ -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
trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_sphere.cpp
+3 -2
View File
@@ -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
trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_trimesh.cpp
+46 -21
View File
@@ -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;
trunk/unmanaged/ubODE-OpenSim/ode/src/collision_util.cpp
+5 -10
View File
@@ -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;
}
trunk/unmanaged/ubODE-OpenSim/ode/src/odemath.cpp
+11
View File
@@ -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)
trunk/unmanaged/ubODE-OpenSim/ode/src/odemath.h
+3 -1
View File
@@ -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)
trunk/unmanaged/ubODE-OpenSim/ode/src/plane.cpp
+74 -55
View File
@@ -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;
}
trunk/unmanaged/ubODE-OpenSim/ode/src/quickstep.cpp
+2 -2
View File
@@ -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);
}
}
}
trunk/unmanaged/ubODE-OpenSim/ode/src/ray.cpp
+172 -126
View File
@@ -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;
trunk/unmanaged/ubODE-OpenSim/ode/src/sphere.cpp
+1 -1
View File
@@ -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;