[llvm-commits] [test-suite] r91782 [2/5] - in /test-suite/trunk/MultiSource/Benchmarks: ./ Bullet/ Bullet/include/ Bullet/include/BulletCollision/ Bullet/include/BulletCollision/BroadphaseCollision/ Bullet/include/BulletCollision/CollisionDispatch/ Bullet/include/BulletCollision/CollisionShapes/ Bullet/include/BulletCollision/Gimpact/ Bullet/include/BulletCollision/NarrowPhaseCollision/ Bullet/include/BulletDynamics/ Bullet/include/BulletDynamics/Character/ Bullet/include/BulletDynamics/ConstraintSolver/ Bullet/include...
Anton Korobeynikov
asl at math.spbu.ru
Sat Dec 19 12:06:02 PST 2009
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btSequentialImpulseConstraintSolver.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btSequentialImpulseConstraintSolver.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btSequentialImpulseConstraintSolver.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btSequentialImpulseConstraintSolver.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,1148 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+//#define COMPUTE_IMPULSE_DENOM 1
+//It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms.
+
+#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
+#include "LinearMath/btIDebugDraw.h"
+#include "BulletDynamics/ConstraintSolver/btJacobianEntry.h"
+#include "LinearMath/btMinMax.h"
+#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
+#include <new>
+#include "LinearMath/btStackAlloc.h"
+#include "LinearMath/btQuickprof.h"
+
+#include "BulletDynamics/ConstraintSolver/btSolverBody.h"
+#include "BulletDynamics/ConstraintSolver/btSolverConstraint.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include <string.h> //for memset
+
+int gNumSplitImpulseRecoveries = 0;
+
+btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
+:m_btSeed2(0)
+{
+
+}
+
+btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
+{
+}
+
+#ifdef USE_SIMD
+#include <emmintrin.h>
+#define vec_splat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e,e,e,e))
+static inline __m128 _vmathVfDot3( __m128 vec0, __m128 vec1 )
+{
+ __m128 result = _mm_mul_ps( vec0, vec1);
+ return _mm_add_ps( vec_splat( result, 0 ), _mm_add_ps( vec_splat( result, 1 ), vec_splat( result, 2 ) ) );
+}
+#endif//USE_SIMD
+
+// Project Gauss Seidel or the equivalent Sequential Impulse
+void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+{
+#ifdef USE_SIMD
+ __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
+ __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
+ __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
+ __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm)));
+ __m128 deltaVel1Dotn = _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.m_deltaLinearVelocity.mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.m_deltaAngularVelocity.mVec128));
+ __m128 deltaVel2Dotn = _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.m_deltaAngularVelocity.mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.m_deltaLinearVelocity.mVec128));
+ deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
+ deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
+ btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
+ btSimdScalar resultLowerLess,resultUpperLess;
+ resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
+ resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
+ __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
+ deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
+ c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
+ __m128 upperMinApplied = _mm_sub_ps(upperLimit1,cpAppliedImp);
+ deltaImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied) );
+ c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1) );
+ __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.m_invMass.mVec128);
+ __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.m_invMass.mVec128);
+ __m128 impulseMagnitude = deltaImpulse;
+ body1.m_deltaLinearVelocity.mVec128 = _mm_add_ps(body1.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
+ body1.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body1.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
+ body2.m_deltaLinearVelocity.mVec128 = _mm_sub_ps(body2.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
+ body2.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body2.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
+#else
+ resolveSingleConstraintRowGeneric(body1,body2,c);
+#endif
+}
+
+// Project Gauss Seidel or the equivalent Sequential Impulse
+ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+{
+ btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
+ const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.m_deltaLinearVelocity) + c.m_relpos1CrossNormal.dot(body1.m_deltaAngularVelocity);
+ const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.m_deltaLinearVelocity) + c.m_relpos2CrossNormal.dot(body2.m_deltaAngularVelocity);
+
+// const btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn;
+ deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
+ deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv;
+
+ const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
+ if (sum < c.m_lowerLimit)
+ {
+ deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
+ c.m_appliedImpulse = c.m_lowerLimit;
+ }
+ else if (sum > c.m_upperLimit)
+ {
+ deltaImpulse = c.m_upperLimit-c.m_appliedImpulse;
+ c.m_appliedImpulse = c.m_upperLimit;
+ }
+ else
+ {
+ c.m_appliedImpulse = sum;
+ }
+ body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse);
+ body2.applyImpulse(-c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,deltaImpulse);
+}
+
+ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+{
+#ifdef USE_SIMD
+ __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
+ __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
+ __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
+ __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm)));
+ __m128 deltaVel1Dotn = _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.m_deltaLinearVelocity.mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.m_deltaAngularVelocity.mVec128));
+ __m128 deltaVel2Dotn = _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.m_deltaAngularVelocity.mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.m_deltaLinearVelocity.mVec128));
+ deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
+ deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
+ btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
+ btSimdScalar resultLowerLess,resultUpperLess;
+ resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
+ resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
+ __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
+ deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
+ c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
+ __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.m_invMass.mVec128);
+ __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.m_invMass.mVec128);
+ __m128 impulseMagnitude = deltaImpulse;
+ body1.m_deltaLinearVelocity.mVec128 = _mm_add_ps(body1.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
+ body1.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body1.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
+ body2.m_deltaLinearVelocity.mVec128 = _mm_sub_ps(body2.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
+ body2.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body2.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
+#else
+ resolveSingleConstraintRowLowerLimit(body1,body2,c);
+#endif
+}
+
+// Project Gauss Seidel or the equivalent Sequential Impulse
+ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+{
+ btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
+ const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.m_deltaLinearVelocity) + c.m_relpos1CrossNormal.dot(body1.m_deltaAngularVelocity);
+ const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.m_deltaLinearVelocity) + c.m_relpos2CrossNormal.dot(body2.m_deltaAngularVelocity);
+
+ deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
+ deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv;
+ const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
+ if (sum < c.m_lowerLimit)
+ {
+ deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
+ c.m_appliedImpulse = c.m_lowerLimit;
+ }
+ else
+ {
+ c.m_appliedImpulse = sum;
+ }
+ body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse);
+ body2.applyImpulse(-c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,deltaImpulse);
+}
+
+
+void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFriendly(
+ btSolverBody& body1,
+ btSolverBody& body2,
+ const btSolverConstraint& c)
+{
+ if (c.m_rhsPenetration)
+ {
+ gNumSplitImpulseRecoveries++;
+ btScalar deltaImpulse = c.m_rhsPenetration-btScalar(c.m_appliedPushImpulse)*c.m_cfm;
+ const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.m_pushVelocity) + c.m_relpos1CrossNormal.dot(body1.m_turnVelocity);
+ const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.m_pushVelocity) + c.m_relpos2CrossNormal.dot(body2.m_turnVelocity);
+
+ deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
+ deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv;
+ const btScalar sum = btScalar(c.m_appliedPushImpulse) + deltaImpulse;
+ if (sum < c.m_lowerLimit)
+ {
+ deltaImpulse = c.m_lowerLimit-c.m_appliedPushImpulse;
+ c.m_appliedPushImpulse = c.m_lowerLimit;
+ }
+ else
+ {
+ c.m_appliedPushImpulse = sum;
+ }
+ body1.internalApplyPushImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse);
+ body2.internalApplyPushImpulse(-c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,deltaImpulse);
+ }
+}
+
+ void btSequentialImpulseConstraintSolver::resolveSplitPenetrationSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+{
+#ifdef USE_SIMD
+ if (!c.m_rhsPenetration)
+ return;
+
+ gNumSplitImpulseRecoveries++;
+
+ __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedPushImpulse);
+ __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
+ __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
+ __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhsPenetration), _mm_mul_ps(_mm_set1_ps(c.m_appliedPushImpulse),_mm_set1_ps(c.m_cfm)));
+ __m128 deltaVel1Dotn = _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.m_pushVelocity.mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.m_turnVelocity.mVec128));
+ __m128 deltaVel2Dotn = _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.m_turnVelocity.mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.m_pushVelocity.mVec128));
+ deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
+ deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
+ btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
+ btSimdScalar resultLowerLess,resultUpperLess;
+ resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
+ resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
+ __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
+ deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
+ c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
+ __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.m_invMass.mVec128);
+ __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.m_invMass.mVec128);
+ __m128 impulseMagnitude = deltaImpulse;
+ body1.m_pushVelocity.mVec128 = _mm_add_ps(body1.m_pushVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
+ body1.m_turnVelocity.mVec128 = _mm_add_ps(body1.m_turnVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
+ body2.m_pushVelocity.mVec128 = _mm_sub_ps(body2.m_pushVelocity.mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
+ body2.m_turnVelocity.mVec128 = _mm_add_ps(body2.m_turnVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
+#else
+ resolveSplitPenetrationImpulseCacheFriendly(body1,body2,c);
+#endif
+}
+
+
+
+unsigned long btSequentialImpulseConstraintSolver::btRand2()
+{
+ m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff;
+ return m_btSeed2;
+}
+
+
+
+//See ODE: adam's all-int straightforward(?) dRandInt (0..n-1)
+int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
+{
+ // seems good; xor-fold and modulus
+ const unsigned long un = static_cast<unsigned long>(n);
+ unsigned long r = btRand2();
+
+ // note: probably more aggressive than it needs to be -- might be
+ // able to get away without one or two of the innermost branches.
+ if (un <= 0x00010000UL) {
+ r ^= (r >> 16);
+ if (un <= 0x00000100UL) {
+ r ^= (r >> 8);
+ if (un <= 0x00000010UL) {
+ r ^= (r >> 4);
+ if (un <= 0x00000004UL) {
+ r ^= (r >> 2);
+ if (un <= 0x00000002UL) {
+ r ^= (r >> 1);
+ }
+ }
+ }
+ }
+ }
+
+ return (int) (r % un);
+}
+
+
+
+void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject)
+{
+ btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0;
+
+ solverBody->m_deltaLinearVelocity.setValue(0.f,0.f,0.f);
+ solverBody->m_deltaAngularVelocity.setValue(0.f,0.f,0.f);
+ solverBody->m_pushVelocity.setValue(0.f,0.f,0.f);
+ solverBody->m_turnVelocity.setValue(0.f,0.f,0.f);
+
+ if (rb)
+ {
+ solverBody->m_invMass = btVector3(rb->getInvMass(),rb->getInvMass(),rb->getInvMass())*rb->getLinearFactor();
+ solverBody->m_originalBody = rb;
+ solverBody->m_angularFactor = rb->getAngularFactor();
+ } else
+ {
+ solverBody->m_invMass.setValue(0,0,0);
+ solverBody->m_originalBody = 0;
+ solverBody->m_angularFactor.setValue(1,1,1);
+ }
+}
+
+
+
+
+
+btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution)
+{
+ btScalar rest = restitution * -rel_vel;
+ return rest;
+}
+
+
+
+void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection);
+void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection)
+{
+ if (colObj && colObj->hasAnisotropicFriction())
+ {
+ // transform to local coordinates
+ btVector3 loc_lateral = frictionDirection * colObj->getWorldTransform().getBasis();
+ const btVector3& friction_scaling = colObj->getAnisotropicFriction();
+ //apply anisotropic friction
+ loc_lateral *= friction_scaling;
+ // ... and transform it back to global coordinates
+ frictionDirection = colObj->getWorldTransform().getBasis() * loc_lateral;
+ }
+}
+
+
+
+btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation)
+{
+
+
+ btRigidBody* body0=btRigidBody::upcast(colObj0);
+ btRigidBody* body1=btRigidBody::upcast(colObj1);
+
+ btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expand();
+ memset(&solverConstraint,0xff,sizeof(btSolverConstraint));
+ solverConstraint.m_contactNormal = normalAxis;
+
+ solverConstraint.m_solverBodyIdA = solverBodyIdA;
+ solverConstraint.m_solverBodyIdB = solverBodyIdB;
+ solverConstraint.m_frictionIndex = frictionIndex;
+
+ solverConstraint.m_friction = cp.m_combinedFriction;
+ solverConstraint.m_originalContactPoint = 0;
+
+ solverConstraint.m_appliedImpulse = 0.f;
+ solverConstraint.m_appliedPushImpulse = 0.f;
+
+ {
+ btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal);
+ solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
+ solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor() : btVector3(0,0,0);
+ }
+ {
+ btVector3 ftorqueAxis1 = rel_pos2.cross(-solverConstraint.m_contactNormal);
+ solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
+ solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor() : btVector3(0,0,0);
+ }
+
+#ifdef COMPUTE_IMPULSE_DENOM
+ btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal);
+ btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal);
+#else
+ btVector3 vec;
+ btScalar denom0 = 0.f;
+ btScalar denom1 = 0.f;
+ if (body0)
+ {
+ vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
+ denom0 = body0->getInvMass() + normalAxis.dot(vec);
+ }
+ if (body1)
+ {
+ vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
+ denom1 = body1->getInvMass() + normalAxis.dot(vec);
+ }
+
+
+#endif //COMPUTE_IMPULSE_DENOM
+ btScalar denom = relaxation/(denom0+denom1);
+ solverConstraint.m_jacDiagABInv = denom;
+
+#ifdef _USE_JACOBIAN
+ solverConstraint.m_jac = btJacobianEntry (
+ rel_pos1,rel_pos2,solverConstraint.m_contactNormal,
+ body0->getInvInertiaDiagLocal(),
+ body0->getInvMass(),
+ body1->getInvInertiaDiagLocal(),
+ body1->getInvMass());
+#endif //_USE_JACOBIAN
+
+
+ {
+ btScalar rel_vel;
+ btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0?body0->getLinearVelocity():btVector3(0,0,0))
+ + solverConstraint.m_relpos1CrossNormal.dot(body0?body0->getAngularVelocity():btVector3(0,0,0));
+ btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(body1?body1->getLinearVelocity():btVector3(0,0,0))
+ + solverConstraint.m_relpos2CrossNormal.dot(body1?body1->getAngularVelocity():btVector3(0,0,0));
+
+ rel_vel = vel1Dotn+vel2Dotn;
+
+// btScalar positionalError = 0.f;
+
+ btSimdScalar velocityError = - rel_vel;
+ btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
+ solverConstraint.m_rhs = velocityImpulse;
+ solverConstraint.m_cfm = 0.f;
+ solverConstraint.m_lowerLimit = 0;
+ solverConstraint.m_upperLimit = 1e10f;
+ }
+
+ return solverConstraint;
+}
+
+int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body)
+{
+ int solverBodyIdA = -1;
+
+ if (body.getCompanionId() >= 0)
+ {
+ //body has already been converted
+ solverBodyIdA = body.getCompanionId();
+ } else
+ {
+ btRigidBody* rb = btRigidBody::upcast(&body);
+ if (rb && rb->getInvMass())
+ {
+ solverBodyIdA = m_tmpSolverBodyPool.size();
+ btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
+ initSolverBody(&solverBody,&body);
+ body.setCompanionId(solverBodyIdA);
+ } else
+ {
+ return 0;//assume first one is a fixed solver body
+ }
+ }
+ return solverBodyIdA;
+}
+#include <stdio.h>
+
+
+
+void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal)
+{
+ btCollisionObject* colObj0=0,*colObj1=0;
+
+ colObj0 = (btCollisionObject*)manifold->getBody0();
+ colObj1 = (btCollisionObject*)manifold->getBody1();
+
+ int solverBodyIdA=-1;
+ int solverBodyIdB=-1;
+
+ if (manifold->getNumContacts())
+ {
+ solverBodyIdA = getOrInitSolverBody(*colObj0);
+ solverBodyIdB = getOrInitSolverBody(*colObj1);
+ }
+
+ ///avoid collision response between two static objects
+ if (!solverBodyIdA && !solverBodyIdB)
+ return;
+
+ btVector3 rel_pos1;
+ btVector3 rel_pos2;
+ btScalar relaxation;
+
+ for (int j=0;j<manifold->getNumContacts();j++)
+ {
+
+ btManifoldPoint& cp = manifold->getContactPoint(j);
+
+ if (cp.getDistance() <= manifold->getContactProcessingThreshold())
+ {
+
+ const btVector3& pos1 = cp.getPositionWorldOnA();
+ const btVector3& pos2 = cp.getPositionWorldOnB();
+
+ rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
+ rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
+
+
+ relaxation = 1.f;
+ btScalar rel_vel;
+ btVector3 vel;
+
+ int frictionIndex = m_tmpSolverContactConstraintPool.size();
+
+ {
+ btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expand();
+ btRigidBody* rb0 = btRigidBody::upcast(colObj0);
+ btRigidBody* rb1 = btRigidBody::upcast(colObj1);
+
+ solverConstraint.m_solverBodyIdA = solverBodyIdA;
+ solverConstraint.m_solverBodyIdB = solverBodyIdB;
+
+ solverConstraint.m_originalContactPoint = &cp;
+
+ btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
+ solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
+ btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);
+ solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
+ {
+#ifdef COMPUTE_IMPULSE_DENOM
+ btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
+ btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
+#else
+ btVector3 vec;
+ btScalar denom0 = 0.f;
+ btScalar denom1 = 0.f;
+ if (rb0)
+ {
+ vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
+ denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
+ }
+ if (rb1)
+ {
+ vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
+ denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
+ }
+#endif //COMPUTE_IMPULSE_DENOM
+
+ btScalar denom = relaxation/(denom0+denom1);
+ solverConstraint.m_jacDiagABInv = denom;
+ }
+
+ solverConstraint.m_contactNormal = cp.m_normalWorldOnB;
+ solverConstraint.m_relpos1CrossNormal = rel_pos1.cross(cp.m_normalWorldOnB);
+ solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(-cp.m_normalWorldOnB);
+
+
+ btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
+ btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
+
+ vel = vel1 - vel2;
+
+ rel_vel = cp.m_normalWorldOnB.dot(vel);
+
+ btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop;
+
+
+ solverConstraint.m_friction = cp.m_combinedFriction;
+
+ btScalar restitution = 0.f;
+
+ if (cp.m_lifeTime>infoGlobal.m_restingContactRestitutionThreshold)
+ {
+ restitution = 0.f;
+ } else
+ {
+ restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution);
+ if (restitution <= btScalar(0.))
+ {
+ restitution = 0.f;
+ };
+ }
+
+
+ ///warm starting (or zero if disabled)
+ if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+ {
+ solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
+ if (rb0)
+ m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
+ if (rb1)
+ m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse);
+ } else
+ {
+ solverConstraint.m_appliedImpulse = 0.f;
+ }
+
+ solverConstraint.m_appliedPushImpulse = 0.f;
+
+ {
+ btScalar rel_vel;
+ btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rb0?rb0->getLinearVelocity():btVector3(0,0,0))
+ + solverConstraint.m_relpos1CrossNormal.dot(rb0?rb0->getAngularVelocity():btVector3(0,0,0));
+ btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rb1?rb1->getLinearVelocity():btVector3(0,0,0))
+ + solverConstraint.m_relpos2CrossNormal.dot(rb1?rb1->getAngularVelocity():btVector3(0,0,0));
+
+ rel_vel = vel1Dotn+vel2Dotn;
+
+ btScalar positionalError = 0.f;
+ positionalError = -penetration * infoGlobal.m_erp/infoGlobal.m_timeStep;
+ btScalar velocityError = restitution - rel_vel;// * damping;
+ btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
+ btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
+ if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
+ {
+ //combine position and velocity into rhs
+ solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
+ solverConstraint.m_rhsPenetration = 0.f;
+ } else
+ {
+ //split position and velocity into rhs and m_rhsPenetration
+ solverConstraint.m_rhs = velocityImpulse;
+ solverConstraint.m_rhsPenetration = penetrationImpulse;
+ }
+ solverConstraint.m_cfm = 0.f;
+ solverConstraint.m_lowerLimit = 0;
+ solverConstraint.m_upperLimit = 1e10f;
+ }
+
+
+ /////setup the friction constraints
+
+
+
+ if (1)
+ {
+ solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
+ if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
+ {
+ cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
+ btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
+ if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
+ {
+ cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel);
+ if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
+ {
+ cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
+ cp.m_lateralFrictionDir2.normalize();//??
+ applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
+ applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
+ addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+ }
+
+ applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
+ applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
+ addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+ cp.m_lateralFrictionInitialized = true;
+ } else
+ {
+ //re-calculate friction direction every frame, todo: check if this is really needed
+ btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
+ if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
+ {
+ applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
+ applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
+ addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+ }
+
+ applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
+ applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
+ addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+
+ cp.m_lateralFrictionInitialized = true;
+ }
+
+ } else
+ {
+ addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+ if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
+ addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+ }
+
+ if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
+ {
+ {
+ btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
+ if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+ {
+ frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
+ if (rb0)
+ m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
+ if (rb1)
+ m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse);
+ } else
+ {
+ frictionConstraint1.m_appliedImpulse = 0.f;
+ }
+ }
+
+ if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
+ {
+ btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
+ if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+ {
+ frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
+ if (rb0)
+ m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
+ if (rb1)
+ m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse);
+ } else
+ {
+ frictionConstraint2.m_appliedImpulse = 0.f;
+ }
+ }
+ } else
+ {
+ btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
+ frictionConstraint1.m_appliedImpulse = 0.f;
+ if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
+ {
+ btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
+ frictionConstraint2.m_appliedImpulse = 0.f;
+ }
+ }
+ }
+ }
+
+
+ }
+ }
+}
+
+
+btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** /*bodies */,int /*numBodies */,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
+{
+ BT_PROFILE("solveGroupCacheFriendlySetup");
+ (void)stackAlloc;
+ (void)debugDrawer;
+
+
+ if (!(numConstraints + numManifolds))
+ {
+ // printf("empty\n");
+ return 0.f;
+ }
+
+ if (1)
+ {
+ int j;
+ for (j=0;j<numConstraints;j++)
+ {
+ btTypedConstraint* constraint = constraints[j];
+ constraint->buildJacobian();
+ }
+ }
+
+ btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
+ initSolverBody(&fixedBody,0);
+
+ //btRigidBody* rb0=0,*rb1=0;
+
+ //if (1)
+ {
+ {
+
+ int totalNumRows = 0;
+ int i;
+
+ m_tmpConstraintSizesPool.resize(numConstraints);
+ //calculate the total number of contraint rows
+ for (i=0;i<numConstraints;i++)
+ {
+ btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
+ constraints[i]->getInfo1(&info1);
+ totalNumRows += info1.m_numConstraintRows;
+ }
+ m_tmpSolverNonContactConstraintPool.resize(totalNumRows);
+
+
+ ///setup the btSolverConstraints
+ int currentRow = 0;
+
+ for (i=0;i<numConstraints;i++)
+ {
+ const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
+
+ if (info1.m_numConstraintRows)
+ {
+ btAssert(currentRow<totalNumRows);
+
+ btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
+ btTypedConstraint* constraint = constraints[i];
+
+
+
+ btRigidBody& rbA = constraint->getRigidBodyA();
+ btRigidBody& rbB = constraint->getRigidBodyB();
+
+ int solverBodyIdA = getOrInitSolverBody(rbA);
+ int solverBodyIdB = getOrInitSolverBody(rbB);
+
+ btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
+ btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
+
+ int j;
+ for ( j=0;j<info1.m_numConstraintRows;j++)
+ {
+ memset(¤tConstraintRow[j],0,sizeof(btSolverConstraint));
+ currentConstraintRow[j].m_lowerLimit = -FLT_MAX;
+ currentConstraintRow[j].m_upperLimit = FLT_MAX;
+ currentConstraintRow[j].m_appliedImpulse = 0.f;
+ currentConstraintRow[j].m_appliedPushImpulse = 0.f;
+ currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA;
+ currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB;
+ }
+
+ bodyAPtr->m_deltaLinearVelocity.setValue(0.f,0.f,0.f);
+ bodyAPtr->m_deltaAngularVelocity.setValue(0.f,0.f,0.f);
+ bodyBPtr->m_deltaLinearVelocity.setValue(0.f,0.f,0.f);
+ bodyBPtr->m_deltaAngularVelocity.setValue(0.f,0.f,0.f);
+
+
+
+ btTypedConstraint::btConstraintInfo2 info2;
+ info2.fps = 1.f/infoGlobal.m_timeStep;
+ info2.erp = infoGlobal.m_erp;
+ info2.m_J1linearAxis = currentConstraintRow->m_contactNormal;
+ info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal;
+ info2.m_J2linearAxis = 0;
+ info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal;
+ info2.rowskip = sizeof(btSolverConstraint)/sizeof(btScalar);//check this
+ ///the size of btSolverConstraint needs be a multiple of btScalar
+ btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint));
+ info2.m_constraintError = ¤tConstraintRow->m_rhs;
+ info2.cfm = ¤tConstraintRow->m_cfm;
+ info2.m_lowerLimit = ¤tConstraintRow->m_lowerLimit;
+ info2.m_upperLimit = ¤tConstraintRow->m_upperLimit;
+ info2.m_numIterations = infoGlobal.m_numIterations;
+ constraints[i]->getInfo2(&info2);
+
+ ///finalize the constraint setup
+ for ( j=0;j<info1.m_numConstraintRows;j++)
+ {
+ btSolverConstraint& solverConstraint = currentConstraintRow[j];
+
+ {
+ const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
+ solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld()*ftorqueAxis1*constraint->getRigidBodyA().getAngularFactor();
+ }
+ {
+ const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;
+ solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()*ftorqueAxis2*constraint->getRigidBodyB().getAngularFactor();
+ }
+
+ {
+ btVector3 iMJlA = solverConstraint.m_contactNormal*rbA.getInvMass();
+ btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal;
+ btVector3 iMJlB = solverConstraint.m_contactNormal*rbB.getInvMass();//sign of normal?
+ btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal;
+
+ btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal);
+ sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
+ sum += iMJlB.dot(solverConstraint.m_contactNormal);
+ sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
+
+ solverConstraint.m_jacDiagABInv = btScalar(1.)/sum;
+ }
+
+
+ ///fix rhs
+ ///todo: add force/torque accelerators
+ {
+ btScalar rel_vel;
+ btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rbA.getLinearVelocity()) + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity());
+ btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rbB.getLinearVelocity()) + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity());
+
+ rel_vel = vel1Dotn+vel2Dotn;
+
+ btScalar restitution = 0.f;
+ btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2
+ btScalar velocityError = restitution - rel_vel;// * damping;
+ btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
+ btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
+ solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
+ solverConstraint.m_appliedImpulse = 0.f;
+
+ }
+ }
+ }
+ currentRow+=m_tmpConstraintSizesPool[i].m_numConstraintRows;
+ }
+ }
+
+ {
+ int i;
+ btPersistentManifold* manifold = 0;
+// btCollisionObject* colObj0=0,*colObj1=0;
+
+
+ for (i=0;i<numManifolds;i++)
+ {
+ manifold = manifoldPtr[i];
+ convertContact(manifold,infoGlobal);
+ }
+ }
+ }
+
+ btContactSolverInfo info = infoGlobal;
+
+
+
+ int numConstraintPool = m_tmpSolverContactConstraintPool.size();
+ int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
+
+ ///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
+ m_orderTmpConstraintPool.resize(numConstraintPool);
+ m_orderFrictionConstraintPool.resize(numFrictionPool);
+ {
+ int i;
+ for (i=0;i<numConstraintPool;i++)
+ {
+ m_orderTmpConstraintPool[i] = i;
+ }
+ for (i=0;i<numFrictionPool;i++)
+ {
+ m_orderFrictionConstraintPool[i] = i;
+ }
+ }
+
+ return 0.f;
+
+}
+
+btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/)
+{
+ BT_PROFILE("solveGroupCacheFriendlyIterations");
+
+ int numConstraintPool = m_tmpSolverContactConstraintPool.size();
+ int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
+
+ //should traverse the contacts random order...
+ int iteration;
+ {
+ for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
+ {
+
+ int j;
+ if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
+ {
+ if ((iteration & 7) == 0) {
+ for (j=0; j<numConstraintPool; ++j) {
+ int tmp = m_orderTmpConstraintPool[j];
+ int swapi = btRandInt2(j+1);
+ m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
+ m_orderTmpConstraintPool[swapi] = tmp;
+ }
+
+ for (j=0; j<numFrictionPool; ++j) {
+ int tmp = m_orderFrictionConstraintPool[j];
+ int swapi = btRandInt2(j+1);
+ m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
+ m_orderFrictionConstraintPool[swapi] = tmp;
+ }
+ }
+ }
+
+ if (infoGlobal.m_solverMode & SOLVER_SIMD)
+ {
+ ///solve all joint constraints, using SIMD, if available
+ for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
+ {
+ btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
+ resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
+ }
+
+ for (j=0;j<numConstraints;j++)
+ {
+ int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA());
+ int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB());
+ btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
+ btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
+ constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
+ }
+
+ ///solve all contact constraints using SIMD, if available
+ int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+ for (j=0;j<numPoolConstraints;j++)
+ {
+ const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
+ resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+
+ }
+ ///solve all friction constraints, using SIMD, if available
+ int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
+ for (j=0;j<numFrictionPoolConstraints;j++)
+ {
+ btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
+ btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+
+ if (totalImpulse>btScalar(0))
+ {
+ solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
+ solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
+
+ resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+ }
+ }
+ } else
+ {
+
+ ///solve all joint constraints
+ for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
+ {
+ btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
+ resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
+ }
+
+ for (j=0;j<numConstraints;j++)
+ {
+ int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA());
+ int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB());
+ btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
+ btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
+
+ constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
+ }
+
+ ///solve all contact constraints
+ int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+ for (j=0;j<numPoolConstraints;j++)
+ {
+ const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
+ resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+ }
+ ///solve all friction constraints
+ int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
+ for (j=0;j<numFrictionPoolConstraints;j++)
+ {
+ btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
+ btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+
+ if (totalImpulse>btScalar(0))
+ {
+ solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
+ solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
+
+ resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+ }
+ }
+ }
+
+ }
+
+ if (infoGlobal.m_splitImpulse)
+ {
+ if (infoGlobal.m_solverMode & SOLVER_SIMD)
+ {
+ for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
+ {
+ {
+ int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+ int j;
+ for (j=0;j<numPoolConstraints;j++)
+ {
+ const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
+
+ resolveSplitPenetrationSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
+ m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+ }
+ }
+ }
+ }
+ else
+ {
+ for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
+ {
+ {
+ int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+ int j;
+ for (j=0;j<numPoolConstraints;j++)
+ {
+ const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
+
+ resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
+ m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+ }
+ }
+ }
+ }
+ }
+
+ }
+ return 0.f;
+}
+
+
+
+/// btSequentialImpulseConstraintSolver Sequentially applies impulses
+btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/)
+{
+
+
+
+ BT_PROFILE("solveGroup");
+ //we only implement SOLVER_CACHE_FRIENDLY now
+ //you need to provide at least some bodies
+ btAssert(bodies);
+ btAssert(numBodies);
+
+ int i;
+
+ solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
+ solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
+
+ int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+ int j;
+
+ for (j=0;j<numPoolConstraints;j++)
+ {
+
+ const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j];
+ btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
+ btAssert(pt);
+ pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
+ if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
+ {
+ pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+ pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
+ }
+
+ //do a callback here?
+ }
+
+ if (infoGlobal.m_splitImpulse)
+ {
+ for ( i=0;i<m_tmpSolverBodyPool.size();i++)
+ {
+ m_tmpSolverBodyPool[i].writebackVelocity(infoGlobal.m_timeStep);
+ }
+ } else
+ {
+ for ( i=0;i<m_tmpSolverBodyPool.size();i++)
+ {
+ m_tmpSolverBodyPool[i].writebackVelocity();
+ }
+ }
+
+
+ m_tmpSolverBodyPool.resize(0);
+ m_tmpSolverContactConstraintPool.resize(0);
+ m_tmpSolverNonContactConstraintPool.resize(0);
+ m_tmpSolverContactFrictionConstraintPool.resize(0);
+
+ return 0.f;
+}
+
+
+
+
+
+
+
+
+
+void btSequentialImpulseConstraintSolver::reset()
+{
+ m_btSeed2 = 0;
+}
+
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btShapeHull.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btShapeHull.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btShapeHull.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btShapeHull.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,165 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+//btShapeHull was implemented by John McCutchan.
+
+
+#include "BulletCollision/CollisionShapes/btShapeHull.h"
+#include "LinearMath/btConvexHull.h"
+
+#define NUM_UNITSPHERE_POINTS 42
+
+static btVector3 btUnitSpherePoints[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] =
+{
+ btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)),
+ btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)),
+ btVector3(btScalar(-0.276388) , btScalar(-0.850649),btScalar(-0.447219)),
+ btVector3(btScalar(-0.894426) , btScalar(-0.000000),btScalar(-0.447216)),
+ btVector3(btScalar(-0.276388) , btScalar(0.850649),btScalar(-0.447220)),
+ btVector3(btScalar(0.723608) , btScalar(0.525725),btScalar(-0.447219)),
+ btVector3(btScalar(0.276388) , btScalar(-0.850649),btScalar(0.447220)),
+ btVector3(btScalar(-0.723608) , btScalar(-0.525725),btScalar(0.447219)),
+ btVector3(btScalar(-0.723608) , btScalar(0.525725),btScalar(0.447219)),
+ btVector3(btScalar(0.276388) , btScalar(0.850649),btScalar(0.447219)),
+ btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)),
+ btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)),
+ btVector3(btScalar(0.425323) , btScalar(-0.309011),btScalar(-0.850654)),
+ btVector3(btScalar(-0.162456) , btScalar(-0.499995),btScalar(-0.850654)),
+ btVector3(btScalar(0.262869) , btScalar(-0.809012),btScalar(-0.525738)),
+ btVector3(btScalar(0.425323) , btScalar(0.309011),btScalar(-0.850654)),
+ btVector3(btScalar(0.850648) , btScalar(-0.000000),btScalar(-0.525736)),
+ btVector3(btScalar(-0.525730) , btScalar(-0.000000),btScalar(-0.850652)),
+ btVector3(btScalar(-0.688190) , btScalar(-0.499997),btScalar(-0.525736)),
+ btVector3(btScalar(-0.162456) , btScalar(0.499995),btScalar(-0.850654)),
+ btVector3(btScalar(-0.688190) , btScalar(0.499997),btScalar(-0.525736)),
+ btVector3(btScalar(0.262869) , btScalar(0.809012),btScalar(-0.525738)),
+ btVector3(btScalar(0.951058) , btScalar(0.309013),btScalar(0.000000)),
+ btVector3(btScalar(0.951058) , btScalar(-0.309013),btScalar(0.000000)),
+ btVector3(btScalar(0.587786) , btScalar(-0.809017),btScalar(0.000000)),
+ btVector3(btScalar(0.000000) , btScalar(-1.000000),btScalar(0.000000)),
+ btVector3(btScalar(-0.587786) , btScalar(-0.809017),btScalar(0.000000)),
+ btVector3(btScalar(-0.951058) , btScalar(-0.309013),btScalar(-0.000000)),
+ btVector3(btScalar(-0.951058) , btScalar(0.309013),btScalar(-0.000000)),
+ btVector3(btScalar(-0.587786) , btScalar(0.809017),btScalar(-0.000000)),
+ btVector3(btScalar(-0.000000) , btScalar(1.000000),btScalar(-0.000000)),
+ btVector3(btScalar(0.587786) , btScalar(0.809017),btScalar(-0.000000)),
+ btVector3(btScalar(0.688190) , btScalar(-0.499997),btScalar(0.525736)),
+ btVector3(btScalar(-0.262869) , btScalar(-0.809012),btScalar(0.525738)),
+ btVector3(btScalar(-0.850648) , btScalar(0.000000),btScalar(0.525736)),
+ btVector3(btScalar(-0.262869) , btScalar(0.809012),btScalar(0.525738)),
+ btVector3(btScalar(0.688190) , btScalar(0.499997),btScalar(0.525736)),
+ btVector3(btScalar(0.525730) , btScalar(0.000000),btScalar(0.850652)),
+ btVector3(btScalar(0.162456) , btScalar(-0.499995),btScalar(0.850654)),
+ btVector3(btScalar(-0.425323) , btScalar(-0.309011),btScalar(0.850654)),
+ btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)),
+ btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654))
+};
+
+btShapeHull::btShapeHull (const btConvexShape* shape)
+{
+ m_shape = shape;
+ m_vertices.clear ();
+ m_indices.clear();
+ m_numIndices = 0;
+}
+
+btShapeHull::~btShapeHull ()
+{
+ m_indices.clear();
+ m_vertices.clear ();
+}
+
+bool
+btShapeHull::buildHull (btScalar /*margin*/)
+{
+ int numSampleDirections = NUM_UNITSPHERE_POINTS;
+ {
+ int numPDA = m_shape->getNumPreferredPenetrationDirections();
+ if (numPDA)
+ {
+ for (int i=0;i<numPDA;i++)
+ {
+ btVector3 norm;
+ m_shape->getPreferredPenetrationDirection(i,norm);
+ btUnitSpherePoints[numSampleDirections] = norm;
+ numSampleDirections++;
+ }
+ }
+ }
+
+ btVector3 supportPoints[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
+ int i;
+ for (i = 0; i < numSampleDirections; i++)
+ {
+ supportPoints[i] = m_shape->localGetSupportingVertex(btUnitSpherePoints[i]);
+ }
+
+ HullDesc hd;
+ hd.mFlags = QF_TRIANGLES;
+ hd.mVcount = static_cast<unsigned int>(numSampleDirections);
+
+#ifdef BT_USE_DOUBLE_PRECISION
+ hd.mVertices = &supportPoints[0];
+ hd.mVertexStride = sizeof(btVector3);
+#else
+ hd.mVertices = &supportPoints[0];
+ hd.mVertexStride = sizeof (btVector3);
+#endif
+
+ HullLibrary hl;
+ HullResult hr;
+ if (hl.CreateConvexHull (hd, hr) == QE_FAIL)
+ {
+ return false;
+ }
+
+ m_vertices.resize (static_cast<int>(hr.mNumOutputVertices));
+
+
+ for (i = 0; i < static_cast<int>(hr.mNumOutputVertices); i++)
+ {
+ m_vertices[i] = hr.m_OutputVertices[i];
+ }
+ m_numIndices = hr.mNumIndices;
+ m_indices.resize(static_cast<int>(m_numIndices));
+ for (i = 0; i < static_cast<int>(m_numIndices); i++)
+ {
+ m_indices[i] = hr.m_Indices[i];
+ }
+
+ // free temporary hull result that we just copied
+ hl.ReleaseResult (hr);
+
+ return true;
+}
+
+int
+btShapeHull::numTriangles () const
+{
+ return static_cast<int>(m_numIndices / 3);
+}
+
+int
+btShapeHull::numVertices () const
+{
+ return m_vertices.size ();
+}
+
+int
+btShapeHull::numIndices () const
+{
+ return static_cast<int>(m_numIndices);
+}
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btSimpleBroadphase.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btSimpleBroadphase.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btSimpleBroadphase.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btSimpleBroadphase.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,330 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
+#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btMatrix3x3.h"
+#include <new>
+
+extern int gOverlappingPairs;
+
+void btSimpleBroadphase::validate()
+{
+ for (int i=0;i<m_numHandles;i++)
+ {
+ for (int j=i+1;j<m_numHandles;j++)
+ {
+ btAssert(&m_pHandles[i] != &m_pHandles[j]);
+ }
+ }
+
+}
+
+btSimpleBroadphase::btSimpleBroadphase(int maxProxies, btOverlappingPairCache* overlappingPairCache)
+ :m_pairCache(overlappingPairCache),
+ m_ownsPairCache(false),
+ m_invalidPair(0)
+{
+
+ if (!overlappingPairCache)
+ {
+ void* mem = btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16);
+ m_pairCache = new (mem)btHashedOverlappingPairCache();
+ m_ownsPairCache = true;
+ }
+
+ // allocate handles buffer and put all handles on free list
+ m_pHandlesRawPtr = btAlignedAlloc(sizeof(btSimpleBroadphaseProxy)*maxProxies,16);
+ m_pHandles = new(m_pHandlesRawPtr) btSimpleBroadphaseProxy[maxProxies];
+ m_maxHandles = maxProxies;
+ m_numHandles = 0;
+ m_firstFreeHandle = 0;
+ m_LastHandleIndex = -1;
+
+
+ {
+ for (int i = m_firstFreeHandle; i < maxProxies; i++)
+ {
+ m_pHandles[i].SetNextFree(i + 1);
+ m_pHandles[i].m_uniqueId = i+2;//any UID will do, we just avoid too trivial values (0,1) for debugging purposes
+ }
+ m_pHandles[maxProxies - 1].SetNextFree(0);
+
+ }
+
+}
+
+btSimpleBroadphase::~btSimpleBroadphase()
+{
+ btAlignedFree(m_pHandlesRawPtr);
+
+ if (m_ownsPairCache)
+ {
+ m_pairCache->~btOverlappingPairCache();
+ btAlignedFree(m_pairCache);
+ }
+}
+
+
+btBroadphaseProxy* btSimpleBroadphase::createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* /*dispatcher*/,void* multiSapProxy)
+{
+ if (m_numHandles >= m_maxHandles)
+ {
+ btAssert(0);
+ return 0; //should never happen, but don't let the game crash ;-)
+ }
+ btAssert(aabbMin[0]<= aabbMax[0] && aabbMin[1]<= aabbMax[1] && aabbMin[2]<= aabbMax[2]);
+
+ int newHandleIndex = allocHandle();
+ btSimpleBroadphaseProxy* proxy = new (&m_pHandles[newHandleIndex])btSimpleBroadphaseProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask,multiSapProxy);
+
+ return proxy;
+}
+
+class RemovingOverlapCallback : public btOverlapCallback
+{
+protected:
+ virtual bool processOverlap(btBroadphasePair& pair)
+ {
+ (void)pair;
+ btAssert(0);
+ return false;
+ }
+};
+
+class RemovePairContainingProxy
+{
+
+ btBroadphaseProxy* m_targetProxy;
+ public:
+ virtual ~RemovePairContainingProxy()
+ {
+ }
+protected:
+ virtual bool processOverlap(btBroadphasePair& pair)
+ {
+ btSimpleBroadphaseProxy* proxy0 = static_cast<btSimpleBroadphaseProxy*>(pair.m_pProxy0);
+ btSimpleBroadphaseProxy* proxy1 = static_cast<btSimpleBroadphaseProxy*>(pair.m_pProxy1);
+
+ return ((m_targetProxy == proxy0 || m_targetProxy == proxy1));
+ };
+};
+
+void btSimpleBroadphase::destroyProxy(btBroadphaseProxy* proxyOrg,btDispatcher* dispatcher)
+{
+
+ btSimpleBroadphaseProxy* proxy0 = static_cast<btSimpleBroadphaseProxy*>(proxyOrg);
+ freeHandle(proxy0);
+
+ m_pairCache->removeOverlappingPairsContainingProxy(proxyOrg,dispatcher);
+
+ //validate();
+
+}
+
+void btSimpleBroadphase::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const
+{
+ const btSimpleBroadphaseProxy* sbp = getSimpleProxyFromProxy(proxy);
+ aabbMin = sbp->m_aabbMin;
+ aabbMax = sbp->m_aabbMax;
+}
+
+void btSimpleBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* /*dispatcher*/)
+{
+ btSimpleBroadphaseProxy* sbp = getSimpleProxyFromProxy(proxy);
+ sbp->m_aabbMin = aabbMin;
+ sbp->m_aabbMax = aabbMax;
+}
+
+void btSimpleBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin,const btVector3& aabbMax)
+{
+ for (int i=0; i <= m_LastHandleIndex; i++)
+ {
+ btSimpleBroadphaseProxy* proxy = &m_pHandles[i];
+ if(!proxy->m_clientObject)
+ {
+ continue;
+ }
+ rayCallback.process(proxy);
+ }
+}
+
+
+
+
+
+
+
+bool btSimpleBroadphase::aabbOverlap(btSimpleBroadphaseProxy* proxy0,btSimpleBroadphaseProxy* proxy1)
+{
+ return proxy0->m_aabbMin[0] <= proxy1->m_aabbMax[0] && proxy1->m_aabbMin[0] <= proxy0->m_aabbMax[0] &&
+ proxy0->m_aabbMin[1] <= proxy1->m_aabbMax[1] && proxy1->m_aabbMin[1] <= proxy0->m_aabbMax[1] &&
+ proxy0->m_aabbMin[2] <= proxy1->m_aabbMax[2] && proxy1->m_aabbMin[2] <= proxy0->m_aabbMax[2];
+
+}
+
+
+
+//then remove non-overlapping ones
+class CheckOverlapCallback : public btOverlapCallback
+{
+public:
+ virtual bool processOverlap(btBroadphasePair& pair)
+ {
+ return (!btSimpleBroadphase::aabbOverlap(static_cast<btSimpleBroadphaseProxy*>(pair.m_pProxy0),static_cast<btSimpleBroadphaseProxy*>(pair.m_pProxy1)));
+ }
+};
+
+void btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
+{
+ //first check for new overlapping pairs
+ int i,j;
+ if (m_numHandles >= 0)
+ {
+ int new_largest_index = -1;
+ for (i=0; i <= m_LastHandleIndex; i++)
+ {
+ btSimpleBroadphaseProxy* proxy0 = &m_pHandles[i];
+ if(!proxy0->m_clientObject)
+ {
+ continue;
+ }
+ new_largest_index = i;
+ for (j=i+1; j <= m_LastHandleIndex; j++)
+ {
+ btSimpleBroadphaseProxy* proxy1 = &m_pHandles[j];
+ btAssert(proxy0 != proxy1);
+ if(!proxy1->m_clientObject)
+ {
+ continue;
+ }
+
+ btSimpleBroadphaseProxy* p0 = getSimpleProxyFromProxy(proxy0);
+ btSimpleBroadphaseProxy* p1 = getSimpleProxyFromProxy(proxy1);
+
+ if (aabbOverlap(p0,p1))
+ {
+ if ( !m_pairCache->findPair(proxy0,proxy1))
+ {
+ m_pairCache->addOverlappingPair(proxy0,proxy1);
+ }
+ } else
+ {
+ if (!m_pairCache->hasDeferredRemoval())
+ {
+ if ( m_pairCache->findPair(proxy0,proxy1))
+ {
+ m_pairCache->removeOverlappingPair(proxy0,proxy1,dispatcher);
+ }
+ }
+ }
+ }
+ }
+
+ m_LastHandleIndex = new_largest_index;
+
+ if (m_ownsPairCache && m_pairCache->hasDeferredRemoval())
+ {
+
+ btBroadphasePairArray& overlappingPairArray = m_pairCache->getOverlappingPairArray();
+
+ //perform a sort, to find duplicates and to sort 'invalid' pairs to the end
+ overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
+
+ overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
+ m_invalidPair = 0;
+
+
+ btBroadphasePair previousPair;
+ previousPair.m_pProxy0 = 0;
+ previousPair.m_pProxy1 = 0;
+ previousPair.m_algorithm = 0;
+
+
+ for (i=0;i<overlappingPairArray.size();i++)
+ {
+
+ btBroadphasePair& pair = overlappingPairArray[i];
+
+ bool isDuplicate = (pair == previousPair);
+
+ previousPair = pair;
+
+ bool needsRemoval = false;
+
+ if (!isDuplicate)
+ {
+ bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1);
+
+ if (hasOverlap)
+ {
+ needsRemoval = false;//callback->processOverlap(pair);
+ } else
+ {
+ needsRemoval = true;
+ }
+ } else
+ {
+ //remove duplicate
+ needsRemoval = true;
+ //should have no algorithm
+ btAssert(!pair.m_algorithm);
+ }
+
+ if (needsRemoval)
+ {
+ m_pairCache->cleanOverlappingPair(pair,dispatcher);
+
+ // m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
+ // m_overlappingPairArray.pop_back();
+ pair.m_pProxy0 = 0;
+ pair.m_pProxy1 = 0;
+ m_invalidPair++;
+ gOverlappingPairs--;
+ }
+
+ }
+
+ ///if you don't like to skip the invalid pairs in the array, execute following code:
+#define CLEAN_INVALID_PAIRS 1
+#ifdef CLEAN_INVALID_PAIRS
+
+ //perform a sort, to sort 'invalid' pairs to the end
+ overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
+
+ overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
+ m_invalidPair = 0;
+#endif//CLEAN_INVALID_PAIRS
+
+ }
+ }
+}
+
+
+bool btSimpleBroadphase::testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+{
+ btSimpleBroadphaseProxy* p0 = getSimpleProxyFromProxy(proxy0);
+ btSimpleBroadphaseProxy* p1 = getSimpleProxyFromProxy(proxy1);
+ return aabbOverlap(p0,p1);
+}
+
+void btSimpleBroadphase::resetPool(btDispatcher* dispatcher)
+{
+ //not yet
+}
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btSimpleDynamicsWorld.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btSimpleDynamicsWorld.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btSimpleDynamicsWorld.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btSimpleDynamicsWorld.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,253 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "BulletDynamics/Dynamics/btSimpleDynamicsWorld.h"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
+#include "BulletCollision/CollisionShapes/btCollisionShape.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
+#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
+
+
+/*
+ Make sure this dummy function never changes so that it
+ can be used by probes that are checking whether the
+ library is actually installed.
+*/
+extern "C"
+{
+ void btBulletDynamicsProbe ();
+ void btBulletDynamicsProbe () {}
+}
+
+
+
+
+btSimpleDynamicsWorld::btSimpleDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration)
+:btDynamicsWorld(dispatcher,pairCache,collisionConfiguration),
+m_constraintSolver(constraintSolver),
+m_ownsConstraintSolver(false),
+m_gravity(0,0,-10)
+{
+
+}
+
+
+btSimpleDynamicsWorld::~btSimpleDynamicsWorld()
+{
+ if (m_ownsConstraintSolver)
+ btAlignedFree( m_constraintSolver);
+}
+
+int btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep)
+{
+ (void)fixedTimeStep;
+ (void)maxSubSteps;
+
+
+ ///apply gravity, predict motion
+ predictUnconstraintMotion(timeStep);
+
+ btDispatcherInfo& dispatchInfo = getDispatchInfo();
+ dispatchInfo.m_timeStep = timeStep;
+ dispatchInfo.m_stepCount = 0;
+ dispatchInfo.m_debugDraw = getDebugDrawer();
+
+ ///perform collision detection
+ performDiscreteCollisionDetection();
+
+ ///solve contact constraints
+ int numManifolds = m_dispatcher1->getNumManifolds();
+ if (numManifolds)
+ {
+ btPersistentManifold** manifoldPtr = ((btCollisionDispatcher*)m_dispatcher1)->getInternalManifoldPointer();
+
+ btContactSolverInfo infoGlobal;
+ infoGlobal.m_timeStep = timeStep;
+ m_constraintSolver->prepareSolve(0,numManifolds);
+ m_constraintSolver->solveGroup(0,0,manifoldPtr, numManifolds,0,0,infoGlobal,m_debugDrawer, m_stackAlloc,m_dispatcher1);
+ m_constraintSolver->allSolved(infoGlobal,m_debugDrawer, m_stackAlloc);
+ }
+
+ ///integrate transforms
+ integrateTransforms(timeStep);
+
+ updateAabbs();
+
+ synchronizeMotionStates();
+
+ clearForces();
+
+ return 1;
+
+}
+
+void btSimpleDynamicsWorld::clearForces()
+{
+ ///@todo: iterate over awake simulation islands!
+ for ( int i=0;i<m_collisionObjects.size();i++)
+ {
+ btCollisionObject* colObj = m_collisionObjects[i];
+
+ btRigidBody* body = btRigidBody::upcast(colObj);
+ if (body)
+ {
+ body->clearForces();
+ }
+ }
+}
+
+
+void btSimpleDynamicsWorld::setGravity(const btVector3& gravity)
+{
+ m_gravity = gravity;
+ for ( int i=0;i<m_collisionObjects.size();i++)
+ {
+ btCollisionObject* colObj = m_collisionObjects[i];
+ btRigidBody* body = btRigidBody::upcast(colObj);
+ if (body)
+ {
+ body->setGravity(gravity);
+ }
+ }
+}
+
+btVector3 btSimpleDynamicsWorld::getGravity () const
+{
+ return m_gravity;
+}
+
+void btSimpleDynamicsWorld::removeRigidBody(btRigidBody* body)
+{
+ btCollisionWorld::removeCollisionObject(body);
+}
+
+void btSimpleDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject)
+{
+ btRigidBody* body = btRigidBody::upcast(collisionObject);
+ if (body)
+ removeRigidBody(body);
+ else
+ btCollisionWorld::removeCollisionObject(collisionObject);
+}
+
+
+void btSimpleDynamicsWorld::addRigidBody(btRigidBody* body)
+{
+ body->setGravity(m_gravity);
+
+ if (body->getCollisionShape())
+ {
+ addCollisionObject(body);
+ }
+}
+
+void btSimpleDynamicsWorld::updateAabbs()
+{
+ btTransform predictedTrans;
+ for ( int i=0;i<m_collisionObjects.size();i++)
+ {
+ btCollisionObject* colObj = m_collisionObjects[i];
+ btRigidBody* body = btRigidBody::upcast(colObj);
+ if (body)
+ {
+ if (body->isActive() && (!body->isStaticObject()))
+ {
+ btVector3 minAabb,maxAabb;
+ colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
+ btBroadphaseInterface* bp = getBroadphase();
+ bp->setAabb(body->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1);
+ }
+ }
+ }
+}
+
+void btSimpleDynamicsWorld::integrateTransforms(btScalar timeStep)
+{
+ btTransform predictedTrans;
+ for ( int i=0;i<m_collisionObjects.size();i++)
+ {
+ btCollisionObject* colObj = m_collisionObjects[i];
+ btRigidBody* body = btRigidBody::upcast(colObj);
+ if (body)
+ {
+ if (body->isActive() && (!body->isStaticObject()))
+ {
+ body->predictIntegratedTransform(timeStep, predictedTrans);
+ body->proceedToTransform( predictedTrans);
+ }
+ }
+ }
+}
+
+
+
+void btSimpleDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
+{
+ for ( int i=0;i<m_collisionObjects.size();i++)
+ {
+ btCollisionObject* colObj = m_collisionObjects[i];
+ btRigidBody* body = btRigidBody::upcast(colObj);
+ if (body)
+ {
+ if (!body->isStaticObject())
+ {
+ if (body->isActive())
+ {
+ body->applyGravity();
+ body->integrateVelocities( timeStep);
+ body->applyDamping(timeStep);
+ body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());
+ }
+ }
+ }
+ }
+}
+
+
+void btSimpleDynamicsWorld::synchronizeMotionStates()
+{
+ ///@todo: iterate over awake simulation islands!
+ for ( int i=0;i<m_collisionObjects.size();i++)
+ {
+ btCollisionObject* colObj = m_collisionObjects[i];
+ btRigidBody* body = btRigidBody::upcast(colObj);
+ if (body && body->getMotionState())
+ {
+ if (body->getActivationState() != ISLAND_SLEEPING)
+ {
+ body->getMotionState()->setWorldTransform(body->getWorldTransform());
+ }
+ }
+ }
+
+}
+
+
+void btSimpleDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
+{
+ if (m_ownsConstraintSolver)
+ {
+ btAlignedFree(m_constraintSolver);
+ }
+ m_ownsConstraintSolver = false;
+ m_constraintSolver = solver;
+}
+
+btConstraintSolver* btSimpleDynamicsWorld::getConstraintSolver()
+{
+ return m_constraintSolver;
+}
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btSimulationIslandManager.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btSimulationIslandManager.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btSimulationIslandManager.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btSimulationIslandManager.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,390 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "LinearMath/btScalar.h"
+#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
+#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
+
+//#include <stdio.h>
+#include "LinearMath/btQuickprof.h"
+
+btSimulationIslandManager::btSimulationIslandManager():
+m_splitIslands(true)
+{
+}
+
+btSimulationIslandManager::~btSimulationIslandManager()
+{
+}
+
+
+void btSimulationIslandManager::initUnionFind(int n)
+{
+ m_unionFind.reset(n);
+}
+
+
+void btSimulationIslandManager::findUnions(btDispatcher* /* dispatcher */,btCollisionWorld* colWorld)
+{
+
+ {
+
+ for (int i=0;i<colWorld->getPairCache()->getNumOverlappingPairs();i++)
+ {
+ btBroadphasePair* pairPtr = colWorld->getPairCache()->getOverlappingPairArrayPtr();
+ const btBroadphasePair& collisionPair = pairPtr[i];
+ btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject;
+ btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject;
+
+ if (((colObj0) && ((colObj0)->mergesSimulationIslands())) &&
+ ((colObj1) && ((colObj1)->mergesSimulationIslands())))
+ {
+
+ m_unionFind.unite((colObj0)->getIslandTag(),
+ (colObj1)->getIslandTag());
+ }
+ }
+ }
+}
+
+
+void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher)
+{
+
+ initUnionFind( int (colWorld->getCollisionObjectArray().size()));
+
+ // put the index into m_controllers into m_tag
+ {
+
+ int index = 0;
+ int i;
+ for (i=0;i<colWorld->getCollisionObjectArray().size(); i++)
+ {
+ btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i];
+ collisionObject->setIslandTag(index);
+ collisionObject->setCompanionId(-1);
+ collisionObject->setHitFraction(btScalar(1.));
+ index++;
+
+ }
+ }
+ // do the union find
+
+ findUnions(dispatcher,colWorld);
+
+
+
+}
+
+
+
+
+void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld)
+{
+ // put the islandId ('find' value) into m_tag
+ {
+
+
+ int index = 0;
+ int i;
+ for (i=0;i<colWorld->getCollisionObjectArray().size();i++)
+ {
+ btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i];
+ if (!collisionObject->isStaticOrKinematicObject())
+ {
+ collisionObject->setIslandTag( m_unionFind.find(index) );
+ collisionObject->setCompanionId(-1);
+ } else
+ {
+ collisionObject->setIslandTag(-1);
+ collisionObject->setCompanionId(-2);
+ }
+ index++;
+ }
+ }
+}
+
+inline int getIslandId(const btPersistentManifold* lhs)
+{
+ int islandId;
+ const btCollisionObject* rcolObj0 = static_cast<const btCollisionObject*>(lhs->getBody0());
+ const btCollisionObject* rcolObj1 = static_cast<const btCollisionObject*>(lhs->getBody1());
+ islandId= rcolObj0->getIslandTag()>=0?rcolObj0->getIslandTag():rcolObj1->getIslandTag();
+ return islandId;
+
+}
+
+
+
+/// function object that routes calls to operator<
+class btPersistentManifoldSortPredicate
+{
+ public:
+
+ SIMD_FORCE_INLINE bool operator() ( const btPersistentManifold* lhs, const btPersistentManifold* rhs )
+ {
+ return getIslandId(lhs) < getIslandId(rhs);
+ }
+};
+
+
+void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld)
+{
+
+ BT_PROFILE("islandUnionFindAndQuickSort");
+
+ btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
+
+ m_islandmanifold.resize(0);
+
+ //we are going to sort the unionfind array, and store the element id in the size
+ //afterwards, we clean unionfind, to make sure no-one uses it anymore
+
+ getUnionFind().sortIslands();
+ int numElem = getUnionFind().getNumElements();
+
+ int endIslandIndex=1;
+ int startIslandIndex;
+
+
+ //update the sleeping state for bodies, if all are sleeping
+ for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
+ {
+ int islandId = getUnionFind().getElement(startIslandIndex).m_id;
+ for (endIslandIndex = startIslandIndex+1;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
+ {
+ }
+
+ //int numSleeping = 0;
+
+ bool allSleeping = true;
+
+ int idx;
+ for (idx=startIslandIndex;idx<endIslandIndex;idx++)
+ {
+ int i = getUnionFind().getElement(idx).m_sz;
+
+ btCollisionObject* colObj0 = collisionObjects[i];
+ if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
+ {
+// printf("error in island management\n");
+ }
+
+ btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+ if (colObj0->getIslandTag() == islandId)
+ {
+ if (colObj0->getActivationState()== ACTIVE_TAG)
+ {
+ allSleeping = false;
+ }
+ if (colObj0->getActivationState()== DISABLE_DEACTIVATION)
+ {
+ allSleeping = false;
+ }
+ }
+ }
+
+
+ if (allSleeping)
+ {
+ int idx;
+ for (idx=startIslandIndex;idx<endIslandIndex;idx++)
+ {
+ int i = getUnionFind().getElement(idx).m_sz;
+ btCollisionObject* colObj0 = collisionObjects[i];
+ if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
+ {
+// printf("error in island management\n");
+ }
+
+ btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+
+ if (colObj0->getIslandTag() == islandId)
+ {
+ colObj0->setActivationState( ISLAND_SLEEPING );
+ }
+ }
+ } else
+ {
+
+ int idx;
+ for (idx=startIslandIndex;idx<endIslandIndex;idx++)
+ {
+ int i = getUnionFind().getElement(idx).m_sz;
+
+ btCollisionObject* colObj0 = collisionObjects[i];
+ if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
+ {
+// printf("error in island management\n");
+ }
+
+ btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+
+ if (colObj0->getIslandTag() == islandId)
+ {
+ if ( colObj0->getActivationState() == ISLAND_SLEEPING)
+ {
+ colObj0->setActivationState( WANTS_DEACTIVATION);
+ colObj0->setDeactivationTime(0.f);
+ }
+ }
+ }
+ }
+ }
+
+
+ int i;
+ int maxNumManifolds = dispatcher->getNumManifolds();
+
+//#define SPLIT_ISLANDS 1
+//#ifdef SPLIT_ISLANDS
+
+
+//#endif //SPLIT_ISLANDS
+
+
+ for (i=0;i<maxNumManifolds ;i++)
+ {
+ btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i);
+
+ btCollisionObject* colObj0 = static_cast<btCollisionObject*>(manifold->getBody0());
+ btCollisionObject* colObj1 = static_cast<btCollisionObject*>(manifold->getBody1());
+
+ ///@todo: check sleeping conditions!
+ if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) ||
+ ((colObj1) && colObj1->getActivationState() != ISLAND_SLEEPING))
+ {
+
+ //kinematic objects don't merge islands, but wake up all connected objects
+ if (colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING)
+ {
+ colObj1->activate();
+ }
+ if (colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING)
+ {
+ colObj0->activate();
+ }
+ if(m_splitIslands)
+ {
+ //filtering for response
+ if (dispatcher->needsResponse(colObj0,colObj1))
+ m_islandmanifold.push_back(manifold);
+ }
+ }
+ }
+}
+
+
+
+///@todo: this is random access, it can be walked 'cache friendly'!
+void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld, IslandCallback* callback)
+{
+ btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
+
+ buildIslands(dispatcher,collisionWorld);
+
+ int endIslandIndex=1;
+ int startIslandIndex;
+ int numElem = getUnionFind().getNumElements();
+
+ BT_PROFILE("processIslands");
+
+ if(!m_splitIslands)
+ {
+ btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer();
+ int maxNumManifolds = dispatcher->getNumManifolds();
+ callback->ProcessIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, -1);
+ }
+ else
+ {
+ // Sort manifolds, based on islands
+ // Sort the vector using predicate and std::sort
+ //std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate);
+
+ int numManifolds = int (m_islandmanifold.size());
+
+ //we should do radix sort, it it much faster (O(n) instead of O (n log2(n))
+ m_islandmanifold.quickSort(btPersistentManifoldSortPredicate());
+
+ //now process all active islands (sets of manifolds for now)
+
+ int startManifoldIndex = 0;
+ int endManifoldIndex = 1;
+
+ //int islandId;
+
+
+
+ // printf("Start Islands\n");
+
+ //traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated
+ for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
+ {
+ int islandId = getUnionFind().getElement(startIslandIndex).m_id;
+
+
+ bool islandSleeping = false;
+
+ for (endIslandIndex = startIslandIndex;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
+ {
+ int i = getUnionFind().getElement(endIslandIndex).m_sz;
+ btCollisionObject* colObj0 = collisionObjects[i];
+ m_islandBodies.push_back(colObj0);
+ if (!colObj0->isActive())
+ islandSleeping = true;
+ }
+
+
+ //find the accompanying contact manifold for this islandId
+ int numIslandManifolds = 0;
+ btPersistentManifold** startManifold = 0;
+
+ if (startManifoldIndex<numManifolds)
+ {
+ int curIslandId = getIslandId(m_islandmanifold[startManifoldIndex]);
+ if (curIslandId == islandId)
+ {
+ startManifold = &m_islandmanifold[startManifoldIndex];
+
+ for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == getIslandId(m_islandmanifold[endManifoldIndex]));endManifoldIndex++)
+ {
+
+ }
+ /// Process the actual simulation, only if not sleeping/deactivated
+ numIslandManifolds = endManifoldIndex-startManifoldIndex;
+ }
+
+ }
+
+ if (!islandSleeping)
+ {
+ callback->ProcessIsland(&m_islandBodies[0],m_islandBodies.size(),startManifold,numIslandManifolds, islandId);
+ // printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds);
+ }
+
+ if (numIslandManifolds)
+ {
+ startManifoldIndex = endManifoldIndex;
+ }
+
+ m_islandBodies.resize(0);
+ }
+ } // else if(!splitIslands)
+
+}
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btSliderConstraint.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btSliderConstraint.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btSliderConstraint.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btSliderConstraint.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,851 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+/*
+Added by Roman Ponomarev (rponom at gmail.com)
+April 04, 2008
+*/
+
+
+
+#include "BulletDynamics/ConstraintSolver/btSliderConstraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btTransformUtil.h"
+#include <new>
+
+
+
+void btSliderConstraint::initParams()
+{
+ m_lowerLinLimit = btScalar(1.0);
+ m_upperLinLimit = btScalar(-1.0);
+ m_lowerAngLimit = btScalar(0.);
+ m_upperAngLimit = btScalar(0.);
+ m_softnessDirLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
+ m_restitutionDirLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
+ m_dampingDirLin = btScalar(0.);
+ m_softnessDirAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
+ m_restitutionDirAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
+ m_dampingDirAng = btScalar(0.);
+ m_softnessOrthoLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
+ m_restitutionOrthoLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
+ m_dampingOrthoLin = SLIDER_CONSTRAINT_DEF_DAMPING;
+ m_softnessOrthoAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
+ m_restitutionOrthoAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
+ m_dampingOrthoAng = SLIDER_CONSTRAINT_DEF_DAMPING;
+ m_softnessLimLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
+ m_restitutionLimLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
+ m_dampingLimLin = SLIDER_CONSTRAINT_DEF_DAMPING;
+ m_softnessLimAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
+ m_restitutionLimAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
+ m_dampingLimAng = SLIDER_CONSTRAINT_DEF_DAMPING;
+
+ m_poweredLinMotor = false;
+ m_targetLinMotorVelocity = btScalar(0.);
+ m_maxLinMotorForce = btScalar(0.);
+ m_accumulatedLinMotorImpulse = btScalar(0.0);
+
+ m_poweredAngMotor = false;
+ m_targetAngMotorVelocity = btScalar(0.);
+ m_maxAngMotorForce = btScalar(0.);
+ m_accumulatedAngMotorImpulse = btScalar(0.0);
+
+}
+
+
+
+btSliderConstraint::btSliderConstraint()
+ :btTypedConstraint(SLIDER_CONSTRAINT_TYPE),
+ m_useSolveConstraintObsolete(false),
+ m_useLinearReferenceFrameA(true)
+{
+ initParams();
+}
+
+
+
+btSliderConstraint::btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
+ : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, rbA, rbB),
+ m_useSolveConstraintObsolete(false),
+ m_frameInA(frameInA),
+ m_frameInB(frameInB),
+ m_useLinearReferenceFrameA(useLinearReferenceFrameA)
+{
+ initParams();
+}
+
+
+static btRigidBody s_fixed(0, 0, 0);
+btSliderConstraint::btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB)
+ : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, s_fixed, rbB),
+ m_useSolveConstraintObsolete(false),
+ m_frameInB(frameInB),
+ m_useLinearReferenceFrameA(useLinearReferenceFrameB)
+{
+ ///not providing rigidbody B means implicitly using worldspace for body B
+// m_frameInA.getOrigin() = m_rbA.getCenterOfMassTransform()(m_frameInA.getOrigin());
+
+ initParams();
+}
+
+
+
+void btSliderConstraint::buildJacobian()
+{
+ if (!m_useSolveConstraintObsolete)
+ {
+ return;
+ }
+ if(m_useLinearReferenceFrameA)
+ {
+ buildJacobianInt(m_rbA, m_rbB, m_frameInA, m_frameInB);
+ }
+ else
+ {
+ buildJacobianInt(m_rbB, m_rbA, m_frameInB, m_frameInA);
+ }
+}
+
+
+
+void btSliderConstraint::buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB)
+{
+#ifndef __SPU__
+ //calculate transforms
+ m_calculatedTransformA = rbA.getCenterOfMassTransform() * frameInA;
+ m_calculatedTransformB = rbB.getCenterOfMassTransform() * frameInB;
+ m_realPivotAInW = m_calculatedTransformA.getOrigin();
+ m_realPivotBInW = m_calculatedTransformB.getOrigin();
+ m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X
+ m_delta = m_realPivotBInW - m_realPivotAInW;
+ m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis;
+ m_relPosA = m_projPivotInW - rbA.getCenterOfMassPosition();
+ m_relPosB = m_realPivotBInW - rbB.getCenterOfMassPosition();
+ btVector3 normalWorld;
+ int i;
+ //linear part
+ for(i = 0; i < 3; i++)
+ {
+ normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
+ new (&m_jacLin[i]) btJacobianEntry(
+ rbA.getCenterOfMassTransform().getBasis().transpose(),
+ rbB.getCenterOfMassTransform().getBasis().transpose(),
+ m_relPosA,
+ m_relPosB,
+ normalWorld,
+ rbA.getInvInertiaDiagLocal(),
+ rbA.getInvMass(),
+ rbB.getInvInertiaDiagLocal(),
+ rbB.getInvMass()
+ );
+ m_jacLinDiagABInv[i] = btScalar(1.) / m_jacLin[i].getDiagonal();
+ m_depth[i] = m_delta.dot(normalWorld);
+ }
+ testLinLimits();
+ // angular part
+ for(i = 0; i < 3; i++)
+ {
+ normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
+ new (&m_jacAng[i]) btJacobianEntry(
+ normalWorld,
+ rbA.getCenterOfMassTransform().getBasis().transpose(),
+ rbB.getCenterOfMassTransform().getBasis().transpose(),
+ rbA.getInvInertiaDiagLocal(),
+ rbB.getInvInertiaDiagLocal()
+ );
+ }
+ testAngLimits();
+ btVector3 axisA = m_calculatedTransformA.getBasis().getColumn(0);
+ m_kAngle = btScalar(1.0 )/ (rbA.computeAngularImpulseDenominator(axisA) + rbB.computeAngularImpulseDenominator(axisA));
+ // clear accumulator for motors
+ m_accumulatedLinMotorImpulse = btScalar(0.0);
+ m_accumulatedAngMotorImpulse = btScalar(0.0);
+#endif //__SPU__
+}
+
+
+void btSliderConstraint::getInfo1(btConstraintInfo1* info)
+{
+ if (m_useSolveConstraintObsolete)
+ {
+ info->m_numConstraintRows = 0;
+ info->nub = 0;
+ }
+ else
+ {
+ info->m_numConstraintRows = 4; // Fixed 2 linear + 2 angular
+ info->nub = 2;
+ //prepare constraint
+ calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+ testLinLimits();
+ if(getSolveLinLimit() || getPoweredLinMotor())
+ {
+ info->m_numConstraintRows++; // limit 3rd linear as well
+ info->nub--;
+ }
+ testAngLimits();
+ if(getSolveAngLimit() || getPoweredAngMotor())
+ {
+ info->m_numConstraintRows++; // limit 3rd angular as well
+ info->nub--;
+ }
+ }
+}
+
+void btSliderConstraint::getInfo1NonVirtual(btConstraintInfo1* info)
+{
+
+ info->m_numConstraintRows = 6; // Fixed 2 linear + 2 angular + 1 limit (even if not used)
+ info->nub = 0;
+}
+
+void btSliderConstraint::getInfo2(btConstraintInfo2* info)
+{
+ getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(), m_rbA.getLinearVelocity(),m_rbB.getLinearVelocity(), m_rbA.getInvMass(),m_rbB.getInvMass());
+}
+
+void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB, const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass )
+{
+ //prepare constraint
+ calculateTransforms(transA,transB);
+ testLinLimits();
+ testAngLimits();
+
+ const btTransform& trA = getCalculatedTransformA();
+ const btTransform& trB = getCalculatedTransformB();
+
+ btAssert(!m_useSolveConstraintObsolete);
+ int i, s = info->rowskip;
+
+ btScalar signFact = m_useLinearReferenceFrameA ? btScalar(1.0f) : btScalar(-1.0f);
+ // make rotations around Y and Z equal
+ // the slider axis should be the only unconstrained
+ // rotational axis, the angular velocity of the two bodies perpendicular to
+ // the slider axis should be equal. thus the constraint equations are
+ // p*w1 - p*w2 = 0
+ // q*w1 - q*w2 = 0
+ // where p and q are unit vectors normal to the slider axis, and w1 and w2
+ // are the angular velocity vectors of the two bodies.
+ // get slider axis (X)
+ btVector3 ax1 = trA.getBasis().getColumn(0);
+ // get 2 orthos to slider axis (Y, Z)
+ btVector3 p = trA.getBasis().getColumn(1);
+ btVector3 q = trA.getBasis().getColumn(2);
+ // set the two slider rows
+ info->m_J1angularAxis[0] = p[0];
+ info->m_J1angularAxis[1] = p[1];
+ info->m_J1angularAxis[2] = p[2];
+ info->m_J1angularAxis[s+0] = q[0];
+ info->m_J1angularAxis[s+1] = q[1];
+ info->m_J1angularAxis[s+2] = q[2];
+
+ info->m_J2angularAxis[0] = -p[0];
+ info->m_J2angularAxis[1] = -p[1];
+ info->m_J2angularAxis[2] = -p[2];
+ info->m_J2angularAxis[s+0] = -q[0];
+ info->m_J2angularAxis[s+1] = -q[1];
+ info->m_J2angularAxis[s+2] = -q[2];
+ // compute the right hand side of the constraint equation. set relative
+ // body velocities along p and q to bring the slider back into alignment.
+ // if ax1,ax2 are the unit length slider axes as computed from body1 and
+ // body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
+ // if "theta" is the angle between ax1 and ax2, we need an angular velocity
+ // along u to cover angle erp*theta in one step :
+ // |angular_velocity| = angle/time = erp*theta / stepsize
+ // = (erp*fps) * theta
+ // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
+ // = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
+ // ...as ax1 and ax2 are unit length. if theta is smallish,
+ // theta ~= sin(theta), so
+ // angular_velocity = (erp*fps) * (ax1 x ax2)
+ // ax1 x ax2 is in the plane space of ax1, so we project the angular
+ // velocity to p and q to find the right hand side.
+ btScalar k = info->fps * info->erp * getSoftnessOrthoAng();
+ btVector3 ax2 = trB.getBasis().getColumn(0);
+ btVector3 u = ax1.cross(ax2);
+ info->m_constraintError[0] = k * u.dot(p);
+ info->m_constraintError[s] = k * u.dot(q);
+ // pull out pos and R for both bodies. also get the connection
+ // vector c = pos2-pos1.
+ // next two rows. we want: vel2 = vel1 + w1 x c ... but this would
+ // result in three equations, so we project along the planespace vectors
+ // so that sliding along the slider axis is disregarded. for symmetry we
+ // also consider rotation around center of mass of two bodies (factA and factB).
+ btTransform bodyA_trans = transA;
+ btTransform bodyB_trans = transB;
+ int s2 = 2 * s, s3 = 3 * s;
+ btVector3 c;
+ btScalar miA = rbAinvMass;
+ btScalar miB = rbBinvMass;
+ btScalar miS = miA + miB;
+ btScalar factA, factB;
+ if(miS > btScalar(0.f))
+ {
+ factA = miB / miS;
+ }
+ else
+ {
+ factA = btScalar(0.5f);
+ }
+ if(factA > 0.99f) factA = 0.99f;
+ if(factA < 0.01f) factA = 0.01f;
+ factB = btScalar(1.0f) - factA;
+ c = bodyB_trans.getOrigin() - bodyA_trans.getOrigin();
+ btVector3 tmp = c.cross(p);
+ for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = factA*tmp[i];
+ for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = factB*tmp[i];
+ tmp = c.cross(q);
+ for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = factA*tmp[i];
+ for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = factB*tmp[i];
+
+ for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i];
+ for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i];
+ // compute two elements of right hand side. we want to align the offset
+ // point (in body 2's frame) with the center of body 1.
+ btVector3 ofs; // offset point in global coordinates
+ ofs = trB.getOrigin() - trA.getOrigin();
+ k = info->fps * info->erp * getSoftnessOrthoLin();
+ info->m_constraintError[s2] = k * p.dot(ofs);
+ info->m_constraintError[s3] = k * q.dot(ofs);
+ int nrow = 3; // last filled row
+ int srow;
+ // check linear limits linear
+ btScalar limit_err = btScalar(0.0);
+ int limit = 0;
+ if(getSolveLinLimit())
+ {
+ limit_err = getLinDepth() * signFact;
+ limit = (limit_err > btScalar(0.0)) ? 2 : 1;
+ }
+ int powered = 0;
+ if(getPoweredLinMotor())
+ {
+ powered = 1;
+ }
+ // if the slider has joint limits or motor, add in the extra row
+ if (limit || powered)
+ {
+ nrow++;
+ srow = nrow * info->rowskip;
+ info->m_J1linearAxis[srow+0] = ax1[0];
+ info->m_J1linearAxis[srow+1] = ax1[1];
+ info->m_J1linearAxis[srow+2] = ax1[2];
+ // linear torque decoupling step:
+ //
+ // we have to be careful that the linear constraint forces (+/- ax1) applied to the two bodies
+ // do not create a torque couple. in other words, the points that the
+ // constraint force is applied at must lie along the same ax1 axis.
+ // a torque couple will result in limited slider-jointed free
+ // bodies from gaining angular momentum.
+ // the solution used here is to apply the constraint forces at the center of mass of the two bodies
+ btVector3 ltd; // Linear Torque Decoupling vector (a torque)
+// c = btScalar(0.5) * c;
+ ltd = c.cross(ax1);
+ info->m_J1angularAxis[srow+0] = factA*ltd[0];
+ info->m_J1angularAxis[srow+1] = factA*ltd[1];
+ info->m_J1angularAxis[srow+2] = factA*ltd[2];
+ info->m_J2angularAxis[srow+0] = factB*ltd[0];
+ info->m_J2angularAxis[srow+1] = factB*ltd[1];
+ info->m_J2angularAxis[srow+2] = factB*ltd[2];
+ // right-hand part
+ btScalar lostop = getLowerLinLimit();
+ btScalar histop = getUpperLinLimit();
+ if(limit && (lostop == histop))
+ { // the joint motor is ineffective
+ powered = 0;
+ }
+ info->m_constraintError[srow] = 0.;
+ info->m_lowerLimit[srow] = 0.;
+ info->m_upperLimit[srow] = 0.;
+ if(powered)
+ {
+ info->cfm[nrow] = btScalar(0.0);
+ btScalar tag_vel = getTargetLinMotorVelocity();
+ btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info->fps * info->erp);
+// info->m_constraintError[srow] += mot_fact * getTargetLinMotorVelocity();
+ info->m_constraintError[srow] -= signFact * mot_fact * getTargetLinMotorVelocity();
+ info->m_lowerLimit[srow] += -getMaxLinMotorForce() * info->fps;
+ info->m_upperLimit[srow] += getMaxLinMotorForce() * info->fps;
+ }
+ if(limit)
+ {
+ k = info->fps * info->erp;
+ info->m_constraintError[srow] += k * limit_err;
+ info->cfm[srow] = btScalar(0.0); // stop_cfm;
+ if(lostop == histop)
+ { // limited low and high simultaneously
+ info->m_lowerLimit[srow] = -SIMD_INFINITY;
+ info->m_upperLimit[srow] = SIMD_INFINITY;
+ }
+ else if(limit == 1)
+ { // low limit
+ info->m_lowerLimit[srow] = -SIMD_INFINITY;
+ info->m_upperLimit[srow] = 0;
+ }
+ else
+ { // high limit
+ info->m_lowerLimit[srow] = 0;
+ info->m_upperLimit[srow] = SIMD_INFINITY;
+ }
+ // bounce (we'll use slider parameter abs(1.0 - m_dampingLimLin) for that)
+ btScalar bounce = btFabs(btScalar(1.0) - getDampingLimLin());
+ if(bounce > btScalar(0.0))
+ {
+ btScalar vel = linVelA.dot(ax1);
+ vel -= linVelB.dot(ax1);
+ vel *= signFact;
+ // only apply bounce if the velocity is incoming, and if the
+ // resulting c[] exceeds what we already have.
+ if(limit == 1)
+ { // low limit
+ if(vel < 0)
+ {
+ btScalar newc = -bounce * vel;
+ if (newc > info->m_constraintError[srow])
+ {
+ info->m_constraintError[srow] = newc;
+ }
+ }
+ }
+ else
+ { // high limit - all those computations are reversed
+ if(vel > 0)
+ {
+ btScalar newc = -bounce * vel;
+ if(newc < info->m_constraintError[srow])
+ {
+ info->m_constraintError[srow] = newc;
+ }
+ }
+ }
+ }
+ info->m_constraintError[srow] *= getSoftnessLimLin();
+ } // if(limit)
+ } // if linear limit
+ // check angular limits
+ limit_err = btScalar(0.0);
+ limit = 0;
+ if(getSolveAngLimit())
+ {
+ limit_err = getAngDepth();
+ limit = (limit_err > btScalar(0.0)) ? 1 : 2;
+ }
+ // if the slider has joint limits, add in the extra row
+ powered = 0;
+ if(getPoweredAngMotor())
+ {
+ powered = 1;
+ }
+ if(limit || powered)
+ {
+ nrow++;
+ srow = nrow * info->rowskip;
+ info->m_J1angularAxis[srow+0] = ax1[0];
+ info->m_J1angularAxis[srow+1] = ax1[1];
+ info->m_J1angularAxis[srow+2] = ax1[2];
+
+ info->m_J2angularAxis[srow+0] = -ax1[0];
+ info->m_J2angularAxis[srow+1] = -ax1[1];
+ info->m_J2angularAxis[srow+2] = -ax1[2];
+
+ btScalar lostop = getLowerAngLimit();
+ btScalar histop = getUpperAngLimit();
+ if(limit && (lostop == histop))
+ { // the joint motor is ineffective
+ powered = 0;
+ }
+ if(powered)
+ {
+ info->cfm[srow] = btScalar(0.0);
+ btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * info->erp);
+ info->m_constraintError[srow] = mot_fact * getTargetAngMotorVelocity();
+ info->m_lowerLimit[srow] = -getMaxAngMotorForce() * info->fps;
+ info->m_upperLimit[srow] = getMaxAngMotorForce() * info->fps;
+ }
+ if(limit)
+ {
+ k = info->fps * info->erp;
+ info->m_constraintError[srow] += k * limit_err;
+ info->cfm[srow] = btScalar(0.0); // stop_cfm;
+ if(lostop == histop)
+ {
+ // limited low and high simultaneously
+ info->m_lowerLimit[srow] = -SIMD_INFINITY;
+ info->m_upperLimit[srow] = SIMD_INFINITY;
+ }
+ else if(limit == 1)
+ { // low limit
+ info->m_lowerLimit[srow] = 0;
+ info->m_upperLimit[srow] = SIMD_INFINITY;
+ }
+ else
+ { // high limit
+ info->m_lowerLimit[srow] = -SIMD_INFINITY;
+ info->m_upperLimit[srow] = 0;
+ }
+ // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
+ btScalar bounce = btFabs(btScalar(1.0) - getDampingLimAng());
+ if(bounce > btScalar(0.0))
+ {
+ btScalar vel = m_rbA.getAngularVelocity().dot(ax1);
+ vel -= m_rbB.getAngularVelocity().dot(ax1);
+ // only apply bounce if the velocity is incoming, and if the
+ // resulting c[] exceeds what we already have.
+ if(limit == 1)
+ { // low limit
+ if(vel < 0)
+ {
+ btScalar newc = -bounce * vel;
+ if(newc > info->m_constraintError[srow])
+ {
+ info->m_constraintError[srow] = newc;
+ }
+ }
+ }
+ else
+ { // high limit - all those computations are reversed
+ if(vel > 0)
+ {
+ btScalar newc = -bounce * vel;
+ if(newc < info->m_constraintError[srow])
+ {
+ info->m_constraintError[srow] = newc;
+ }
+ }
+ }
+ }
+ info->m_constraintError[srow] *= getSoftnessLimAng();
+ } // if(limit)
+ } // if angular limit or powered
+}
+
+
+
+void btSliderConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
+{
+ if (m_useSolveConstraintObsolete)
+ {
+ m_timeStep = timeStep;
+ if(m_useLinearReferenceFrameA)
+ {
+ solveConstraintInt(m_rbA,bodyA, m_rbB,bodyB);
+ }
+ else
+ {
+ solveConstraintInt(m_rbB,bodyB, m_rbA,bodyA);
+ }
+ }
+}
+
+
+
+void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB)
+{
+#ifndef __SPU__
+ int i;
+ // linear
+ btVector3 velA;
+ bodyA.getVelocityInLocalPointObsolete(m_relPosA,velA);
+ btVector3 velB;
+ bodyB.getVelocityInLocalPointObsolete(m_relPosB,velB);
+ btVector3 vel = velA - velB;
+ for(i = 0; i < 3; i++)
+ {
+ const btVector3& normal = m_jacLin[i].m_linearJointAxis;
+ btScalar rel_vel = normal.dot(vel);
+ // calculate positional error
+ btScalar depth = m_depth[i];
+ // get parameters
+ btScalar softness = (i) ? m_softnessOrthoLin : (m_solveLinLim ? m_softnessLimLin : m_softnessDirLin);
+ btScalar restitution = (i) ? m_restitutionOrthoLin : (m_solveLinLim ? m_restitutionLimLin : m_restitutionDirLin);
+ btScalar damping = (i) ? m_dampingOrthoLin : (m_solveLinLim ? m_dampingLimLin : m_dampingDirLin);
+ // calcutate and apply impulse
+ btScalar normalImpulse = softness * (restitution * depth / m_timeStep - damping * rel_vel) * m_jacLinDiagABInv[i];
+ btVector3 impulse_vector = normal * normalImpulse;
+
+ //rbA.applyImpulse( impulse_vector, m_relPosA);
+ //rbB.applyImpulse(-impulse_vector, m_relPosB);
+ {
+ btVector3 ftorqueAxis1 = m_relPosA.cross(normal);
+ btVector3 ftorqueAxis2 = m_relPosB.cross(normal);
+ bodyA.applyImpulse(normal*rbA.getInvMass(), rbA.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);
+ bodyB.applyImpulse(normal*rbB.getInvMass(), rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse);
+ }
+
+
+
+ if(m_poweredLinMotor && (!i))
+ { // apply linear motor
+ if(m_accumulatedLinMotorImpulse < m_maxLinMotorForce)
+ {
+ btScalar desiredMotorVel = m_targetLinMotorVelocity;
+ btScalar motor_relvel = desiredMotorVel + rel_vel;
+ normalImpulse = -motor_relvel * m_jacLinDiagABInv[i];
+ // clamp accumulated impulse
+ btScalar new_acc = m_accumulatedLinMotorImpulse + btFabs(normalImpulse);
+ if(new_acc > m_maxLinMotorForce)
+ {
+ new_acc = m_maxLinMotorForce;
+ }
+ btScalar del = new_acc - m_accumulatedLinMotorImpulse;
+ if(normalImpulse < btScalar(0.0))
+ {
+ normalImpulse = -del;
+ }
+ else
+ {
+ normalImpulse = del;
+ }
+ m_accumulatedLinMotorImpulse = new_acc;
+ // apply clamped impulse
+ impulse_vector = normal * normalImpulse;
+ //rbA.applyImpulse( impulse_vector, m_relPosA);
+ //rbB.applyImpulse(-impulse_vector, m_relPosB);
+
+ {
+ btVector3 ftorqueAxis1 = m_relPosA.cross(normal);
+ btVector3 ftorqueAxis2 = m_relPosB.cross(normal);
+ bodyA.applyImpulse(normal*rbA.getInvMass(), rbA.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);
+ bodyB.applyImpulse(normal*rbB.getInvMass(), rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse);
+ }
+
+
+
+ }
+ }
+ }
+ // angular
+ // get axes in world space
+ btVector3 axisA = m_calculatedTransformA.getBasis().getColumn(0);
+ btVector3 axisB = m_calculatedTransformB.getBasis().getColumn(0);
+
+ btVector3 angVelA;
+ bodyA.getAngularVelocity(angVelA);
+ btVector3 angVelB;
+ bodyB.getAngularVelocity(angVelB);
+
+ btVector3 angVelAroundAxisA = axisA * axisA.dot(angVelA);
+ btVector3 angVelAroundAxisB = axisB * axisB.dot(angVelB);
+
+ btVector3 angAorthog = angVelA - angVelAroundAxisA;
+ btVector3 angBorthog = angVelB - angVelAroundAxisB;
+ btVector3 velrelOrthog = angAorthog-angBorthog;
+ //solve orthogonal angular velocity correction
+ btScalar len = velrelOrthog.length();
+ btScalar orthorImpulseMag = 0.f;
+
+ if (len > btScalar(0.00001))
+ {
+ btVector3 normal = velrelOrthog.normalized();
+ btScalar denom = rbA.computeAngularImpulseDenominator(normal) + rbB.computeAngularImpulseDenominator(normal);
+ //velrelOrthog *= (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng;
+ orthorImpulseMag = (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng;
+ }
+ //solve angular positional correction
+ btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/m_timeStep);
+ btVector3 angularAxis = angularError;
+ btScalar angularImpulseMag = 0;
+
+ btScalar len2 = angularError.length();
+ if (len2>btScalar(0.00001))
+ {
+ btVector3 normal2 = angularError.normalized();
+ btScalar denom2 = rbA.computeAngularImpulseDenominator(normal2) + rbB.computeAngularImpulseDenominator(normal2);
+ angularImpulseMag = (btScalar(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng;
+ angularError *= angularImpulseMag;
+ }
+ // apply impulse
+ //rbA.applyTorqueImpulse(-velrelOrthog+angularError);
+ //rbB.applyTorqueImpulse(velrelOrthog-angularError);
+
+ bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*velrelOrthog,-orthorImpulseMag);
+ bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*velrelOrthog,orthorImpulseMag);
+ bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*angularAxis,angularImpulseMag);
+ bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*angularAxis,-angularImpulseMag);
+
+
+ btScalar impulseMag;
+ //solve angular limits
+ if(m_solveAngLim)
+ {
+ impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingLimAng + m_angDepth * m_restitutionLimAng / m_timeStep;
+ impulseMag *= m_kAngle * m_softnessLimAng;
+ }
+ else
+ {
+ impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingDirAng + m_angDepth * m_restitutionDirAng / m_timeStep;
+ impulseMag *= m_kAngle * m_softnessDirAng;
+ }
+ btVector3 impulse = axisA * impulseMag;
+ //rbA.applyTorqueImpulse(impulse);
+ //rbB.applyTorqueImpulse(-impulse);
+
+ bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*axisA,impulseMag);
+ bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*axisA,-impulseMag);
+
+
+
+ //apply angular motor
+ if(m_poweredAngMotor)
+ {
+ if(m_accumulatedAngMotorImpulse < m_maxAngMotorForce)
+ {
+ btVector3 velrel = angVelAroundAxisA - angVelAroundAxisB;
+ btScalar projRelVel = velrel.dot(axisA);
+
+ btScalar desiredMotorVel = m_targetAngMotorVelocity;
+ btScalar motor_relvel = desiredMotorVel - projRelVel;
+
+ btScalar angImpulse = m_kAngle * motor_relvel;
+ // clamp accumulated impulse
+ btScalar new_acc = m_accumulatedAngMotorImpulse + btFabs(angImpulse);
+ if(new_acc > m_maxAngMotorForce)
+ {
+ new_acc = m_maxAngMotorForce;
+ }
+ btScalar del = new_acc - m_accumulatedAngMotorImpulse;
+ if(angImpulse < btScalar(0.0))
+ {
+ angImpulse = -del;
+ }
+ else
+ {
+ angImpulse = del;
+ }
+ m_accumulatedAngMotorImpulse = new_acc;
+ // apply clamped impulse
+ btVector3 motorImp = angImpulse * axisA;
+ //rbA.applyTorqueImpulse(motorImp);
+ //rbB.applyTorqueImpulse(-motorImp);
+
+ bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*axisA,angImpulse);
+ bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*axisA,-angImpulse);
+ }
+ }
+#endif //__SPU__
+}
+
+
+
+
+
+void btSliderConstraint::calculateTransforms(const btTransform& transA,const btTransform& transB)
+{
+ if(m_useLinearReferenceFrameA || (!m_useSolveConstraintObsolete))
+ {
+ m_calculatedTransformA = transA * m_frameInA;
+ m_calculatedTransformB = transB * m_frameInB;
+ }
+ else
+ {
+ m_calculatedTransformA = transB * m_frameInB;
+ m_calculatedTransformB = transA * m_frameInA;
+ }
+ m_realPivotAInW = m_calculatedTransformA.getOrigin();
+ m_realPivotBInW = m_calculatedTransformB.getOrigin();
+ m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X
+ if(m_useLinearReferenceFrameA || m_useSolveConstraintObsolete)
+ {
+ m_delta = m_realPivotBInW - m_realPivotAInW;
+ }
+ else
+ {
+ m_delta = m_realPivotAInW - m_realPivotBInW;
+ }
+ m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis;
+ btVector3 normalWorld;
+ int i;
+ //linear part
+ for(i = 0; i < 3; i++)
+ {
+ normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
+ m_depth[i] = m_delta.dot(normalWorld);
+ }
+}
+
+
+
+void btSliderConstraint::testLinLimits(void)
+{
+ m_solveLinLim = false;
+ m_linPos = m_depth[0];
+ if(m_lowerLinLimit <= m_upperLinLimit)
+ {
+ if(m_depth[0] > m_upperLinLimit)
+ {
+ m_depth[0] -= m_upperLinLimit;
+ m_solveLinLim = true;
+ }
+ else if(m_depth[0] < m_lowerLinLimit)
+ {
+ m_depth[0] -= m_lowerLinLimit;
+ m_solveLinLim = true;
+ }
+ else
+ {
+ m_depth[0] = btScalar(0.);
+ }
+ }
+ else
+ {
+ m_depth[0] = btScalar(0.);
+ }
+}
+
+
+
+void btSliderConstraint::testAngLimits(void)
+{
+ m_angDepth = btScalar(0.);
+ m_solveAngLim = false;
+ if(m_lowerAngLimit <= m_upperAngLimit)
+ {
+ const btVector3 axisA0 = m_calculatedTransformA.getBasis().getColumn(1);
+ const btVector3 axisA1 = m_calculatedTransformA.getBasis().getColumn(2);
+ const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1);
+ btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0));
+ rot = btAdjustAngleToLimits(rot, m_lowerAngLimit, m_upperAngLimit);
+ m_angPos = rot;
+ if(rot < m_lowerAngLimit)
+ {
+ m_angDepth = rot - m_lowerAngLimit;
+ m_solveAngLim = true;
+ }
+ else if(rot > m_upperAngLimit)
+ {
+ m_angDepth = rot - m_upperAngLimit;
+ m_solveAngLim = true;
+ }
+ }
+}
+
+
+
+btVector3 btSliderConstraint::getAncorInA(void)
+{
+ btVector3 ancorInA;
+ ancorInA = m_realPivotAInW + (m_lowerLinLimit + m_upperLinLimit) * btScalar(0.5) * m_sliderAxis;
+ ancorInA = m_rbA.getCenterOfMassTransform().inverse() * ancorInA;
+ return ancorInA;
+}
+
+
+
+btVector3 btSliderConstraint::getAncorInB(void)
+{
+ btVector3 ancorInB;
+ ancorInB = m_frameInB.getOrigin();
+ return ancorInB;
+}
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btSoftBody.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btSoftBody.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btSoftBody.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btSoftBody.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,2917 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+///btSoftBody implementation by Nathanael Presson
+
+#include "BulletSoftBody/btSoftBodyInternals.h"
+
+//
+btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo,int node_count, const btVector3* x, const btScalar* m)
+:m_worldInfo(worldInfo)
+{
+ /* Init */
+ m_internalType = CO_SOFT_BODY;
+ m_cfg.aeromodel = eAeroModel::V_Point;
+ m_cfg.kVCF = 1;
+ m_cfg.kDG = 0;
+ m_cfg.kLF = 0;
+ m_cfg.kDP = 0;
+ m_cfg.kPR = 0;
+ m_cfg.kVC = 0;
+ m_cfg.kDF = (btScalar)0.2;
+ m_cfg.kMT = 0;
+ m_cfg.kCHR = (btScalar)1.0;
+ m_cfg.kKHR = (btScalar)0.1;
+ m_cfg.kSHR = (btScalar)1.0;
+ m_cfg.kAHR = (btScalar)0.7;
+ m_cfg.kSRHR_CL = (btScalar)0.1;
+ m_cfg.kSKHR_CL = (btScalar)1;
+ m_cfg.kSSHR_CL = (btScalar)0.5;
+ m_cfg.kSR_SPLT_CL = (btScalar)0.5;
+ m_cfg.kSK_SPLT_CL = (btScalar)0.5;
+ m_cfg.kSS_SPLT_CL = (btScalar)0.5;
+ m_cfg.maxvolume = (btScalar)1;
+ m_cfg.timescale = 1;
+ m_cfg.viterations = 0;
+ m_cfg.piterations = 1;
+ m_cfg.diterations = 0;
+ m_cfg.citerations = 4;
+ m_cfg.collisions = fCollision::Default;
+ m_pose.m_bvolume = false;
+ m_pose.m_bframe = false;
+ m_pose.m_volume = 0;
+ m_pose.m_com = btVector3(0,0,0);
+ m_pose.m_rot.setIdentity();
+ m_pose.m_scl.setIdentity();
+ m_tag = 0;
+ m_timeacc = 0;
+ m_bUpdateRtCst = true;
+ m_bounds[0] = btVector3(0,0,0);
+ m_bounds[1] = btVector3(0,0,0);
+ m_worldTransform.setIdentity();
+ setSolver(eSolverPresets::Positions);
+ /* Default material */
+ Material* pm=appendMaterial();
+ pm->m_kLST = 1;
+ pm->m_kAST = 1;
+ pm->m_kVST = 1;
+ pm->m_flags = fMaterial::Default;
+ /* Collision shape */
+ ///for now, create a collision shape internally
+ m_collisionShape = new btSoftBodyCollisionShape(this);
+ m_collisionShape->setMargin(0.25);
+ /* Nodes */
+ const btScalar margin=getCollisionShape()->getMargin();
+ m_nodes.resize(node_count);
+ for(int i=0,ni=node_count;i<ni;++i)
+ {
+ Node& n=m_nodes[i];
+ ZeroInitialize(n);
+ n.m_x = x?*x++:btVector3(0,0,0);
+ n.m_q = n.m_x;
+ n.m_im = m?*m++:1;
+ n.m_im = n.m_im>0?1/n.m_im:0;
+ n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x,margin),&n);
+ n.m_material= pm;
+ }
+ updateBounds();
+
+ m_initialWorldTransform.setIdentity();
+}
+
+//
+btSoftBody::~btSoftBody()
+{
+ //for now, delete the internal shape
+ delete m_collisionShape;
+ int i;
+
+ releaseClusters();
+ for(i=0;i<m_materials.size();++i)
+ btAlignedFree(m_materials[i]);
+ for(i=0;i<m_joints.size();++i)
+ btAlignedFree(m_joints[i]);
+}
+
+//
+bool btSoftBody::checkLink(int node0,int node1) const
+{
+ return(checkLink(&m_nodes[node0],&m_nodes[node1]));
+}
+
+//
+bool btSoftBody::checkLink(const Node* node0,const Node* node1) const
+{
+ const Node* n[]={node0,node1};
+ for(int i=0,ni=m_links.size();i<ni;++i)
+ {
+ const Link& l=m_links[i];
+ if( (l.m_n[0]==n[0]&&l.m_n[1]==n[1])||
+ (l.m_n[0]==n[1]&&l.m_n[1]==n[0]))
+ {
+ return(true);
+ }
+ }
+ return(false);
+}
+
+//
+bool btSoftBody::checkFace(int node0,int node1,int node2) const
+{
+ const Node* n[]={ &m_nodes[node0],
+ &m_nodes[node1],
+ &m_nodes[node2]};
+ for(int i=0,ni=m_faces.size();i<ni;++i)
+ {
+ const Face& f=m_faces[i];
+ int c=0;
+ for(int j=0;j<3;++j)
+ {
+ if( (f.m_n[j]==n[0])||
+ (f.m_n[j]==n[1])||
+ (f.m_n[j]==n[2])) c|=1<<j; else break;
+ }
+ if(c==7) return(true);
+ }
+ return(false);
+}
+
+//
+btSoftBody::Material* btSoftBody::appendMaterial()
+{
+ Material* pm=new(btAlignedAlloc(sizeof(Material),16)) Material();
+ if(m_materials.size()>0)
+ *pm=*m_materials[0];
+ else
+ ZeroInitialize(*pm);
+ m_materials.push_back(pm);
+ return(pm);
+}
+
+//
+void btSoftBody::appendNote( const char* text,
+ const btVector3& o,
+ const btVector4& c,
+ Node* n0,
+ Node* n1,
+ Node* n2,
+ Node* n3)
+{
+ Note n;
+ ZeroInitialize(n);
+ n.m_rank = 0;
+ n.m_text = text;
+ n.m_offset = o;
+ n.m_coords[0] = c.x();
+ n.m_coords[1] = c.y();
+ n.m_coords[2] = c.z();
+ n.m_coords[3] = c.w();
+ n.m_nodes[0] = n0;n.m_rank+=n0?1:0;
+ n.m_nodes[1] = n1;n.m_rank+=n1?1:0;
+ n.m_nodes[2] = n2;n.m_rank+=n2?1:0;
+ n.m_nodes[3] = n3;n.m_rank+=n3?1:0;
+ m_notes.push_back(n);
+}
+
+//
+void btSoftBody::appendNote( const char* text,
+ const btVector3& o,
+ Node* feature)
+{
+ appendNote(text,o,btVector4(1,0,0,0),feature);
+}
+
+//
+void btSoftBody::appendNote( const char* text,
+ const btVector3& o,
+ Link* feature)
+{
+ static const btScalar w=1/(btScalar)2;
+ appendNote(text,o,btVector4(w,w,0,0), feature->m_n[0],
+ feature->m_n[1]);
+}
+
+//
+void btSoftBody::appendNote( const char* text,
+ const btVector3& o,
+ Face* feature)
+{
+ static const btScalar w=1/(btScalar)3;
+ appendNote(text,o,btVector4(w,w,w,0), feature->m_n[0],
+ feature->m_n[1],
+ feature->m_n[2]);
+}
+
+//
+void btSoftBody::appendNode( const btVector3& x,btScalar m)
+{
+ if(m_nodes.capacity()==m_nodes.size())
+ {
+ pointersToIndices();
+ m_nodes.reserve(m_nodes.size()*2+1);
+ indicesToPointers();
+ }
+ const btScalar margin=getCollisionShape()->getMargin();
+ m_nodes.push_back(Node());
+ Node& n=m_nodes[m_nodes.size()-1];
+ ZeroInitialize(n);
+ n.m_x = x;
+ n.m_q = n.m_x;
+ n.m_im = m>0?1/m:0;
+ n.m_material = m_materials[0];
+ n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x,margin),&n);
+}
+
+//
+void btSoftBody::appendLink(int model,Material* mat)
+{
+ Link l;
+ if(model>=0)
+ l=m_links[model];
+ else
+ { ZeroInitialize(l);l.m_material=mat?mat:m_materials[0]; }
+ m_links.push_back(l);
+}
+
+//
+void btSoftBody::appendLink( int node0,
+ int node1,
+ Material* mat,
+ bool bcheckexist)
+{
+ appendLink(&m_nodes[node0],&m_nodes[node1],mat,bcheckexist);
+}
+
+//
+void btSoftBody::appendLink( Node* node0,
+ Node* node1,
+ Material* mat,
+ bool bcheckexist)
+{
+ if((!bcheckexist)||(!checkLink(node0,node1)))
+ {
+ appendLink(-1,mat);
+ Link& l=m_links[m_links.size()-1];
+ l.m_n[0] = node0;
+ l.m_n[1] = node1;
+ l.m_rl = (l.m_n[0]->m_x-l.m_n[1]->m_x).length();
+ m_bUpdateRtCst=true;
+ }
+}
+
+//
+void btSoftBody::appendFace(int model,Material* mat)
+{
+ Face f;
+ if(model>=0)
+ { f=m_faces[model]; }
+ else
+ { ZeroInitialize(f);f.m_material=mat?mat:m_materials[0]; }
+ m_faces.push_back(f);
+}
+
+//
+void btSoftBody::appendFace(int node0,int node1,int node2,Material* mat)
+{
+ if (node0==node1)
+ return;
+ if (node1==node2)
+ return;
+ if (node2==node0)
+ return;
+
+ appendFace(-1,mat);
+ Face& f=m_faces[m_faces.size()-1];
+ btAssert(node0!=node1);
+ btAssert(node1!=node2);
+ btAssert(node2!=node0);
+ f.m_n[0] = &m_nodes[node0];
+ f.m_n[1] = &m_nodes[node1];
+ f.m_n[2] = &m_nodes[node2];
+ f.m_ra = AreaOf( f.m_n[0]->m_x,
+ f.m_n[1]->m_x,
+ f.m_n[2]->m_x);
+ m_bUpdateRtCst=true;
+}
+
+//
+void btSoftBody::appendTetra(int model,Material* mat)
+{
+Tetra t;
+if(model>=0)
+ t=m_tetras[model];
+ else
+ { ZeroInitialize(t);t.m_material=mat?mat:m_materials[0]; }
+m_tetras.push_back(t);
+}
+
+//
+void btSoftBody::appendTetra(int node0,
+ int node1,
+ int node2,
+ int node3,
+ Material* mat)
+{
+ appendTetra(-1,mat);
+ Tetra& t=m_tetras[m_tetras.size()-1];
+ t.m_n[0] = &m_nodes[node0];
+ t.m_n[1] = &m_nodes[node1];
+ t.m_n[2] = &m_nodes[node2];
+ t.m_n[3] = &m_nodes[node3];
+ t.m_rv = VolumeOf(t.m_n[0]->m_x,t.m_n[1]->m_x,t.m_n[2]->m_x,t.m_n[3]->m_x);
+ m_bUpdateRtCst=true;
+}
+
+//
+void btSoftBody::appendAnchor(int node,btRigidBody* body, bool disableCollisionBetweenLinkedBodies)
+{
+ if (disableCollisionBetweenLinkedBodies)
+ {
+ if (m_collisionDisabledObjects.findLinearSearch(body)==m_collisionDisabledObjects.size())
+ {
+ m_collisionDisabledObjects.push_back(body);
+ }
+ }
+
+ Anchor a;
+ a.m_node = &m_nodes[node];
+ a.m_body = body;
+ a.m_local = body->getInterpolationWorldTransform().inverse()*a.m_node->m_x;
+ a.m_node->m_battach = 1;
+ m_anchors.push_back(a);
+}
+
+//
+void btSoftBody::appendLinearJoint(const LJoint::Specs& specs,Cluster* body0,Body body1)
+{
+ LJoint* pj = new(btAlignedAlloc(sizeof(LJoint),16)) LJoint();
+ pj->m_bodies[0] = body0;
+ pj->m_bodies[1] = body1;
+ pj->m_refs[0] = pj->m_bodies[0].xform().inverse()*specs.position;
+ pj->m_refs[1] = pj->m_bodies[1].xform().inverse()*specs.position;
+ pj->m_cfm = specs.cfm;
+ pj->m_erp = specs.erp;
+ pj->m_split = specs.split;
+ m_joints.push_back(pj);
+}
+
+//
+void btSoftBody::appendLinearJoint(const LJoint::Specs& specs,Body body)
+{
+ appendLinearJoint(specs,m_clusters[0],body);
+}
+
+//
+void btSoftBody::appendLinearJoint(const LJoint::Specs& specs,btSoftBody* body)
+{
+ appendLinearJoint(specs,m_clusters[0],body->m_clusters[0]);
+}
+
+//
+void btSoftBody::appendAngularJoint(const AJoint::Specs& specs,Cluster* body0,Body body1)
+{
+ AJoint* pj = new(btAlignedAlloc(sizeof(AJoint),16)) AJoint();
+ pj->m_bodies[0] = body0;
+ pj->m_bodies[1] = body1;
+ pj->m_refs[0] = pj->m_bodies[0].xform().inverse().getBasis()*specs.axis;
+ pj->m_refs[1] = pj->m_bodies[1].xform().inverse().getBasis()*specs.axis;
+ pj->m_cfm = specs.cfm;
+ pj->m_erp = specs.erp;
+ pj->m_split = specs.split;
+ pj->m_icontrol = specs.icontrol;
+ m_joints.push_back(pj);
+}
+
+//
+void btSoftBody::appendAngularJoint(const AJoint::Specs& specs,Body body)
+{
+ appendAngularJoint(specs,m_clusters[0],body);
+}
+
+//
+void btSoftBody::appendAngularJoint(const AJoint::Specs& specs,btSoftBody* body)
+{
+ appendAngularJoint(specs,m_clusters[0],body->m_clusters[0]);
+}
+
+//
+void btSoftBody::addForce(const btVector3& force)
+{
+ for(int i=0,ni=m_nodes.size();i<ni;++i) addForce(force,i);
+}
+
+//
+void btSoftBody::addForce(const btVector3& force,int node)
+{
+ Node& n=m_nodes[node];
+ if(n.m_im>0)
+ {
+ n.m_f += force;
+ }
+}
+
+//
+void btSoftBody::addVelocity(const btVector3& velocity)
+{
+ for(int i=0,ni=m_nodes.size();i<ni;++i) addVelocity(velocity,i);
+}
+
+/* Set velocity for the entire body */
+void btSoftBody::setVelocity( const btVector3& velocity)
+{
+ for(int i=0,ni=m_nodes.size();i<ni;++i)
+ {
+ Node& n=m_nodes[i];
+ if(n.m_im>0)
+ {
+ n.m_v = velocity;
+ }
+ }
+}
+
+
+//
+void btSoftBody::addVelocity(const btVector3& velocity,int node)
+{
+ Node& n=m_nodes[node];
+ if(n.m_im>0)
+ {
+ n.m_v += velocity;
+ }
+}
+
+//
+void btSoftBody::setMass(int node,btScalar mass)
+{
+ m_nodes[node].m_im=mass>0?1/mass:0;
+ m_bUpdateRtCst=true;
+}
+
+//
+btScalar btSoftBody::getMass(int node) const
+{
+ return(m_nodes[node].m_im>0?1/m_nodes[node].m_im:0);
+}
+
+//
+btScalar btSoftBody::getTotalMass() const
+{
+ btScalar mass=0;
+ for(int i=0;i<m_nodes.size();++i)
+ {
+ mass+=getMass(i);
+ }
+ return(mass);
+}
+
+//
+void btSoftBody::setTotalMass(btScalar mass,bool fromfaces)
+{
+ int i;
+
+ if(fromfaces)
+ {
+
+ for(i=0;i<m_nodes.size();++i)
+ {
+ m_nodes[i].m_im=0;
+ }
+ for(i=0;i<m_faces.size();++i)
+ {
+ const Face& f=m_faces[i];
+ const btScalar twicearea=AreaOf( f.m_n[0]->m_x,
+ f.m_n[1]->m_x,
+ f.m_n[2]->m_x);
+ for(int j=0;j<3;++j)
+ {
+ f.m_n[j]->m_im+=twicearea;
+ }
+ }
+ for( i=0;i<m_nodes.size();++i)
+ {
+ m_nodes[i].m_im=1/m_nodes[i].m_im;
+ }
+ }
+ const btScalar tm=getTotalMass();
+ const btScalar itm=1/tm;
+ for( i=0;i<m_nodes.size();++i)
+ {
+ m_nodes[i].m_im/=itm*mass;
+ }
+ m_bUpdateRtCst=true;
+}
+
+//
+void btSoftBody::setTotalDensity(btScalar density)
+{
+ setTotalMass(getVolume()*density,true);
+}
+
+//
+void btSoftBody::setVolumeMass(btScalar mass)
+{
+btAlignedObjectArray<btScalar> ranks;
+ranks.resize(m_nodes.size(),0);
+for(int i=0;i<m_nodes.size();++i)
+ {
+ m_nodes[i].m_im=0;
+ }
+for(int i=0;i<m_tetras.size();++i)
+ {
+ const Tetra& t=m_tetras[i];
+ for(int j=0;j<4;++j)
+ {
+ t.m_n[j]->m_im+=btFabs(t.m_rv);
+ ranks[int(t.m_n[j]-&m_nodes[0])]+=1;
+ }
+ }
+for(int i=0;i<m_nodes.size();++i)
+ {
+ if(m_nodes[i].m_im>0)
+ {
+ m_nodes[i].m_im=ranks[i]/m_nodes[i].m_im;
+ }
+ }
+setTotalMass(mass,false);
+}
+
+//
+void btSoftBody::setVolumeDensity(btScalar density)
+{
+btScalar volume=0;
+for(int i=0;i<m_tetras.size();++i)
+ {
+ const Tetra& t=m_tetras[i];
+ for(int j=0;j<4;++j)
+ {
+ volume+=btFabs(t.m_rv);
+ }
+ }
+setVolumeMass(volume*density/6);
+}
+
+//
+void btSoftBody::transform(const btTransform& trs)
+{
+ const btScalar margin=getCollisionShape()->getMargin();
+ ATTRIBUTE_ALIGNED16(btDbvtVolume) vol;
+
+ for(int i=0,ni=m_nodes.size();i<ni;++i)
+ {
+ Node& n=m_nodes[i];
+ n.m_x=trs*n.m_x;
+ n.m_q=trs*n.m_q;
+ n.m_n=trs.getBasis()*n.m_n;
+ vol = btDbvtVolume::FromCR(n.m_x,margin);
+
+ m_ndbvt.update(n.m_leaf,vol);
+ }
+ updateNormals();
+ updateBounds();
+ updateConstants();
+ m_initialWorldTransform = trs;
+}
+
+//
+void btSoftBody::translate(const btVector3& trs)
+{
+ btTransform t;
+ t.setIdentity();
+ t.setOrigin(trs);
+ transform(t);
+}
+
+//
+void btSoftBody::rotate( const btQuaternion& rot)
+{
+ btTransform t;
+ t.setIdentity();
+ t.setRotation(rot);
+ transform(t);
+}
+
+//
+void btSoftBody::scale(const btVector3& scl)
+{
+ const btScalar margin=getCollisionShape()->getMargin();
+ ATTRIBUTE_ALIGNED16(btDbvtVolume) vol;
+
+ for(int i=0,ni=m_nodes.size();i<ni;++i)
+ {
+ Node& n=m_nodes[i];
+ n.m_x*=scl;
+ n.m_q*=scl;
+ vol = btDbvtVolume::FromCR(n.m_x,margin);
+ m_ndbvt.update(n.m_leaf,vol);
+ }
+ updateNormals();
+ updateBounds();
+ updateConstants();
+}
+
+//
+void btSoftBody::setPose(bool bvolume,bool bframe)
+{
+ m_pose.m_bvolume = bvolume;
+ m_pose.m_bframe = bframe;
+ int i,ni;
+
+ /* Weights */
+ const btScalar omass=getTotalMass();
+ const btScalar kmass=omass*m_nodes.size()*1000;
+ btScalar tmass=omass;
+ m_pose.m_wgh.resize(m_nodes.size());
+ for(i=0,ni=m_nodes.size();i<ni;++i)
+ {
+ if(m_nodes[i].m_im<=0) tmass+=kmass;
+ }
+ for( i=0,ni=m_nodes.size();i<ni;++i)
+ {
+ Node& n=m_nodes[i];
+ m_pose.m_wgh[i]= n.m_im>0 ?
+ 1/(m_nodes[i].m_im*tmass) :
+ kmass/tmass;
+ }
+ /* Pos */
+ const btVector3 com=evaluateCom();
+ m_pose.m_pos.resize(m_nodes.size());
+ for( i=0,ni=m_nodes.size();i<ni;++i)
+ {
+ m_pose.m_pos[i]=m_nodes[i].m_x-com;
+ }
+ m_pose.m_volume = bvolume?getVolume():0;
+ m_pose.m_com = com;
+ m_pose.m_rot.setIdentity();
+ m_pose.m_scl.setIdentity();
+ /* Aqq */
+ m_pose.m_aqq[0] =
+ m_pose.m_aqq[1] =
+ m_pose.m_aqq[2] = btVector3(0,0,0);
+ for( i=0,ni=m_nodes.size();i<ni;++i)
+ {
+ const btVector3& q=m_pose.m_pos[i];
+ const btVector3 mq=m_pose.m_wgh[i]*q;
+ m_pose.m_aqq[0]+=mq.x()*q;
+ m_pose.m_aqq[1]+=mq.y()*q;
+ m_pose.m_aqq[2]+=mq.z()*q;
+ }
+ m_pose.m_aqq=m_pose.m_aqq.inverse();
+ updateConstants();
+}
+
+//
+btScalar btSoftBody::getVolume() const
+{
+ btScalar vol=0;
+ if(m_nodes.size()>0)
+ {
+ int i,ni;
+
+ const btVector3 org=m_nodes[0].m_x;
+ for(i=0,ni=m_faces.size();i<ni;++i)
+ {
+ const Face& f=m_faces[i];
+ vol+=btDot(f.m_n[0]->m_x-org,btCross(f.m_n[1]->m_x-org,f.m_n[2]->m_x-org));
+ }
+ vol/=(btScalar)6;
+ }
+ return(vol);
+}
+
+//
+int btSoftBody::clusterCount() const
+{
+ return(m_clusters.size());
+}
+
+//
+btVector3 btSoftBody::clusterCom(const Cluster* cluster)
+{
+ btVector3 com(0,0,0);
+ for(int i=0,ni=cluster->m_nodes.size();i<ni;++i)
+ {
+ com+=cluster->m_nodes[i]->m_x*cluster->m_masses[i];
+ }
+ return(com*cluster->m_imass);
+}
+
+//
+btVector3 btSoftBody::clusterCom(int cluster) const
+{
+ return(clusterCom(m_clusters[cluster]));
+}
+
+//
+btVector3 btSoftBody::clusterVelocity(const Cluster* cluster,const btVector3& rpos)
+{
+ return(cluster->m_lv+btCross(cluster->m_av,rpos));
+}
+
+//
+void btSoftBody::clusterVImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse)
+{
+ const btVector3 li=cluster->m_imass*impulse;
+ const btVector3 ai=cluster->m_invwi*btCross(rpos,impulse);
+ cluster->m_vimpulses[0]+=li;cluster->m_lv+=li;
+ cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai;
+ cluster->m_nvimpulses++;
+}
+
+//
+void btSoftBody::clusterDImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse)
+{
+ const btVector3 li=cluster->m_imass*impulse;
+ const btVector3 ai=cluster->m_invwi*btCross(rpos,impulse);
+ cluster->m_dimpulses[0]+=li;
+ cluster->m_dimpulses[1]+=ai;
+ cluster->m_ndimpulses++;
+}
+
+//
+void btSoftBody::clusterImpulse(Cluster* cluster,const btVector3& rpos,const Impulse& impulse)
+{
+ if(impulse.m_asVelocity) clusterVImpulse(cluster,rpos,impulse.m_velocity);
+ if(impulse.m_asDrift) clusterDImpulse(cluster,rpos,impulse.m_drift);
+}
+
+//
+void btSoftBody::clusterVAImpulse(Cluster* cluster,const btVector3& impulse)
+{
+ const btVector3 ai=cluster->m_invwi*impulse;
+ cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai;
+ cluster->m_nvimpulses++;
+}
+
+//
+void btSoftBody::clusterDAImpulse(Cluster* cluster,const btVector3& impulse)
+{
+ const btVector3 ai=cluster->m_invwi*impulse;
+ cluster->m_dimpulses[1]+=ai;
+ cluster->m_ndimpulses++;
+}
+
+//
+void btSoftBody::clusterAImpulse(Cluster* cluster,const Impulse& impulse)
+{
+ if(impulse.m_asVelocity) clusterVAImpulse(cluster,impulse.m_velocity);
+ if(impulse.m_asDrift) clusterDAImpulse(cluster,impulse.m_drift);
+}
+
+//
+void btSoftBody::clusterDCImpulse(Cluster* cluster,const btVector3& impulse)
+{
+ cluster->m_dimpulses[0]+=impulse*cluster->m_imass;
+ cluster->m_ndimpulses++;
+}
+
+struct NodeLinks
+{
+ btAlignedObjectArray<int> m_links;
+};
+
+
+
+//
+int btSoftBody::generateBendingConstraints(int distance,Material* mat)
+{
+ int i,j;
+
+ if(distance>1)
+ {
+ /* Build graph */
+ const int n=m_nodes.size();
+ const unsigned inf=(~(unsigned)0)>>1;
+ unsigned* adj=new unsigned[n*n];
+
+
+#define IDX(_x_,_y_) ((_y_)*n+(_x_))
+ for(j=0;j<n;++j)
+ {
+ for(i=0;i<n;++i)
+ {
+ if(i!=j)
+ {
+ adj[IDX(i,j)]=adj[IDX(j,i)]=inf;
+ }
+ else
+ {
+ adj[IDX(i,j)]=adj[IDX(j,i)]=0;
+ }
+ }
+ }
+ for( i=0;i<m_links.size();++i)
+ {
+ const int ia=(int)(m_links[i].m_n[0]-&m_nodes[0]);
+ const int ib=(int)(m_links[i].m_n[1]-&m_nodes[0]);
+ adj[IDX(ia,ib)]=1;
+ adj[IDX(ib,ia)]=1;
+ }
+
+
+ //special optimized case for distance == 2
+ if (distance == 2)
+ {
+
+ btAlignedObjectArray<NodeLinks> nodeLinks;
+
+
+ /* Build node links */
+ nodeLinks.resize(m_nodes.size());
+
+ for( i=0;i<m_links.size();++i)
+ {
+ const int ia=(int)(m_links[i].m_n[0]-&m_nodes[0]);
+ const int ib=(int)(m_links[i].m_n[1]-&m_nodes[0]);
+ if (nodeLinks[ia].m_links.findLinearSearch(ib)==nodeLinks[ia].m_links.size())
+ nodeLinks[ia].m_links.push_back(ib);
+
+ if (nodeLinks[ib].m_links.findLinearSearch(ia)==nodeLinks[ib].m_links.size())
+ nodeLinks[ib].m_links.push_back(ia);
+ }
+ for (int ii=0;ii<nodeLinks.size();ii++)
+ {
+ int i=ii;
+
+ for (int jj=0;jj<nodeLinks[ii].m_links.size();jj++)
+ {
+ int k = nodeLinks[ii].m_links[jj];
+ for (int kk=0;kk<nodeLinks[k].m_links.size();kk++)
+ {
+ int j = nodeLinks[k].m_links[kk];
+ if (i!=j)
+ {
+ const unsigned sum=adj[IDX(i,k)]+adj[IDX(k,j)];
+ btAssert(sum==2);
+ if(adj[IDX(i,j)]>sum)
+ {
+ adj[IDX(i,j)]=adj[IDX(j,i)]=sum;
+ }
+ }
+
+ }
+ }
+ }
+ } else
+ {
+ ///generic Floyd's algorithm
+ for(int k=0;k<n;++k)
+ {
+ for(j=0;j<n;++j)
+ {
+ for(i=j+1;i<n;++i)
+ {
+ const unsigned sum=adj[IDX(i,k)]+adj[IDX(k,j)];
+ if(adj[IDX(i,j)]>sum)
+ {
+ adj[IDX(i,j)]=adj[IDX(j,i)]=sum;
+ }
+ }
+ }
+ }
+ }
+
+
+ /* Build links */
+ int nlinks=0;
+ for(j=0;j<n;++j)
+ {
+ for(i=j+1;i<n;++i)
+ {
+ if(adj[IDX(i,j)]==(unsigned)distance)
+ {
+ appendLink(i,j,mat);
+ m_links[m_links.size()-1].m_bbending=1;
+ ++nlinks;
+ }
+ }
+ }
+ delete[] adj;
+ return(nlinks);
+ }
+ return(0);
+}
+
+//
+void btSoftBody::randomizeConstraints()
+{
+ unsigned long seed=243703;
+#define NEXTRAND (seed=(1664525L*seed+1013904223L)&0xffffffff)
+ int i,ni;
+
+ for(i=0,ni=m_links.size();i<ni;++i)
+ {
+ btSwap(m_links[i],m_links[NEXTRAND%ni]);
+ }
+ for(i=0,ni=m_faces.size();i<ni;++i)
+ {
+ btSwap(m_faces[i],m_faces[NEXTRAND%ni]);
+ }
+#undef NEXTRAND
+}
+
+//
+void btSoftBody::releaseCluster(int index)
+{
+ Cluster* c=m_clusters[index];
+ if(c->m_leaf) m_cdbvt.remove(c->m_leaf);
+ c->~Cluster();
+ btAlignedFree(c);
+ m_clusters.remove(c);
+}
+
+//
+void btSoftBody::releaseClusters()
+{
+ while(m_clusters.size()>0) releaseCluster(0);
+}
+
+//
+int btSoftBody::generateClusters(int k,int maxiterations)
+{
+ int i;
+ releaseClusters();
+ m_clusters.resize(btMin(k,m_nodes.size()));
+ for(i=0;i<m_clusters.size();++i)
+ {
+ m_clusters[i] = new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
+ m_clusters[i]->m_collide= true;
+ }
+ k=m_clusters.size();
+ if(k>0)
+ {
+ /* Initialize */
+ btAlignedObjectArray<btVector3> centers;
+ btVector3 cog(0,0,0);
+ int i;
+ for(i=0;i<m_nodes.size();++i)
+ {
+ cog+=m_nodes[i].m_x;
+ m_clusters[(i*29873)%m_clusters.size()]->m_nodes.push_back(&m_nodes[i]);
+ }
+ cog/=(btScalar)m_nodes.size();
+ centers.resize(k,cog);
+ /* Iterate */
+ const btScalar slope=16;
+ bool changed;
+ int iterations=0;
+ do {
+ const btScalar w=2-btMin<btScalar>(1,iterations/slope);
+ changed=false;
+ iterations++;
+ int i;
+
+ for(i=0;i<k;++i)
+ {
+ btVector3 c(0,0,0);
+ for(int j=0;j<m_clusters[i]->m_nodes.size();++j)
+ {
+ c+=m_clusters[i]->m_nodes[j]->m_x;
+ }
+ if(m_clusters[i]->m_nodes.size())
+ {
+ c /= (btScalar)m_clusters[i]->m_nodes.size();
+ c = centers[i]+(c-centers[i])*w;
+ changed |= ((c-centers[i]).length2()>SIMD_EPSILON);
+ centers[i] = c;
+ m_clusters[i]->m_nodes.resize(0);
+ }
+ }
+ for(i=0;i<m_nodes.size();++i)
+ {
+ const btVector3 nx=m_nodes[i].m_x;
+ int kbest=0;
+ btScalar kdist=ClusterMetric(centers[0],nx);
+ for(int j=1;j<k;++j)
+ {
+ const btScalar d=ClusterMetric(centers[j],nx);
+ if(d<kdist)
+ {
+ kbest=j;
+ kdist=d;
+ }
+ }
+ m_clusters[kbest]->m_nodes.push_back(&m_nodes[i]);
+ }
+ } while(changed&&(iterations<maxiterations));
+ /* Merge */
+ btAlignedObjectArray<int> cids;
+ cids.resize(m_nodes.size(),-1);
+ for(i=0;i<m_clusters.size();++i)
+ {
+ for(int j=0;j<m_clusters[i]->m_nodes.size();++j)
+ {
+ cids[int(m_clusters[i]->m_nodes[j]-&m_nodes[0])]=i;
+ }
+ }
+ for(i=0;i<m_faces.size();++i)
+ {
+ const int idx[]={ int(m_faces[i].m_n[0]-&m_nodes[0]),
+ int(m_faces[i].m_n[1]-&m_nodes[0]),
+ int(m_faces[i].m_n[2]-&m_nodes[0])};
+ for(int j=0;j<3;++j)
+ {
+ const int cid=cids[idx[j]];
+ for(int q=1;q<3;++q)
+ {
+ const int kid=idx[(j+q)%3];
+ if(cids[kid]!=cid)
+ {
+ if(m_clusters[cid]->m_nodes.findLinearSearch(&m_nodes[kid])==m_clusters[cid]->m_nodes.size())
+ {
+ m_clusters[cid]->m_nodes.push_back(&m_nodes[kid]);
+ }
+ }
+ }
+ }
+ }
+ /* Master */
+ if(m_clusters.size()>1)
+ {
+ Cluster* pmaster=new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
+ pmaster->m_collide = false;
+ pmaster->m_nodes.reserve(m_nodes.size());
+ for(int i=0;i<m_nodes.size();++i) pmaster->m_nodes.push_back(&m_nodes[i]);
+ m_clusters.push_back(pmaster);
+ btSwap(m_clusters[0],m_clusters[m_clusters.size()-1]);
+ }
+ /* Terminate */
+ for(i=0;i<m_clusters.size();++i)
+ {
+ if(m_clusters[i]->m_nodes.size()==0)
+ {
+ releaseCluster(i--);
+ }
+ }
+ } else
+ {
+ //create a cluster for each tetrahedron (if tetrahedra exist) or each face
+ if (m_tetras.size())
+ {
+ m_clusters.resize(m_tetras.size());
+ for(i=0;i<m_clusters.size();++i)
+ {
+ m_clusters[i] = new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
+ m_clusters[i]->m_collide= true;
+ }
+ for (i=0;i<m_tetras.size();i++)
+ {
+ for (int j=0;j<4;j++)
+ {
+ m_clusters[i]->m_nodes.push_back(m_tetras[i].m_n[j]);
+ }
+ }
+
+ } else
+ {
+ m_clusters.resize(m_faces.size());
+ for(i=0;i<m_clusters.size();++i)
+ {
+ m_clusters[i] = new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
+ m_clusters[i]->m_collide= true;
+ }
+
+ for(i=0;i<m_faces.size();++i)
+ {
+ for(int j=0;j<3;++j)
+ {
+ m_clusters[i]->m_nodes.push_back(m_faces[i].m_n[j]);
+ }
+ }
+ }
+ }
+
+ if (m_clusters.size())
+ {
+ initializeClusters();
+ updateClusters();
+
+
+ //for self-collision
+ m_clusterConnectivity.resize(m_clusters.size()*m_clusters.size());
+ {
+ for (int c0=0;c0<m_clusters.size();c0++)
+ {
+ m_clusters[c0]->m_clusterIndex=c0;
+ for (int c1=0;c1<m_clusters.size();c1++)
+ {
+
+ bool connected=false;
+ Cluster* cla = m_clusters[c0];
+ Cluster* clb = m_clusters[c1];
+ for (int i=0;!connected&&i<cla->m_nodes.size();i++)
+ {
+ for (int j=0;j<clb->m_nodes.size();j++)
+ {
+ if (cla->m_nodes[i] == clb->m_nodes[j])
+ {
+ connected=true;
+ break;
+ }
+ }
+ }
+ m_clusterConnectivity[c0+c1*m_clusters.size()]=connected;
+ }
+ }
+ }
+ }
+
+ return(m_clusters.size());
+}
+
+//
+void btSoftBody::refine(ImplicitFn* ifn,btScalar accurary,bool cut)
+{
+ const Node* nbase = &m_nodes[0];
+ int ncount = m_nodes.size();
+ btSymMatrix<int> edges(ncount,-2);
+ int newnodes=0;
+ int i,j,k,ni;
+
+ /* Filter out */
+ for(i=0;i<m_links.size();++i)
+ {
+ Link& l=m_links[i];
+ if(l.m_bbending)
+ {
+ if(!SameSign(ifn->Eval(l.m_n[0]->m_x),ifn->Eval(l.m_n[1]->m_x)))
+ {
+ btSwap(m_links[i],m_links[m_links.size()-1]);
+ m_links.pop_back();--i;
+ }
+ }
+ }
+ /* Fill edges */
+ for(i=0;i<m_links.size();++i)
+ {
+ Link& l=m_links[i];
+ edges(int(l.m_n[0]-nbase),int(l.m_n[1]-nbase))=-1;
+ }
+ for(i=0;i<m_faces.size();++i)
+ {
+ Face& f=m_faces[i];
+ edges(int(f.m_n[0]-nbase),int(f.m_n[1]-nbase))=-1;
+ edges(int(f.m_n[1]-nbase),int(f.m_n[2]-nbase))=-1;
+ edges(int(f.m_n[2]-nbase),int(f.m_n[0]-nbase))=-1;
+ }
+ /* Intersect */
+ for(i=0;i<ncount;++i)
+ {
+ for(j=i+1;j<ncount;++j)
+ {
+ if(edges(i,j)==-1)
+ {
+ Node& a=m_nodes[i];
+ Node& b=m_nodes[j];
+ const btScalar t=ImplicitSolve(ifn,a.m_x,b.m_x,accurary);
+ if(t>0)
+ {
+ const btVector3 x=Lerp(a.m_x,b.m_x,t);
+ const btVector3 v=Lerp(a.m_v,b.m_v,t);
+ btScalar m=0;
+ if(a.m_im>0)
+ {
+ if(b.m_im>0)
+ {
+ const btScalar ma=1/a.m_im;
+ const btScalar mb=1/b.m_im;
+ const btScalar mc=Lerp(ma,mb,t);
+ const btScalar f=(ma+mb)/(ma+mb+mc);
+ a.m_im=1/(ma*f);
+ b.m_im=1/(mb*f);
+ m=mc*f;
+ }
+ else
+ { a.m_im/=0.5;m=1/a.m_im; }
+ }
+ else
+ {
+ if(b.m_im>0)
+ { b.m_im/=0.5;m=1/b.m_im; }
+ else
+ m=0;
+ }
+ appendNode(x,m);
+ edges(i,j)=m_nodes.size()-1;
+ m_nodes[edges(i,j)].m_v=v;
+ ++newnodes;
+ }
+ }
+ }
+ }
+ nbase=&m_nodes[0];
+ /* Refine links */
+ for(i=0,ni=m_links.size();i<ni;++i)
+ {
+ Link& feat=m_links[i];
+ const int idx[]={ int(feat.m_n[0]-nbase),
+ int(feat.m_n[1]-nbase)};
+ if((idx[0]<ncount)&&(idx[1]<ncount))
+ {
+ const int ni=edges(idx[0],idx[1]);
+ if(ni>0)
+ {
+ appendLink(i);
+ Link* pft[]={ &m_links[i],
+ &m_links[m_links.size()-1]};
+ pft[0]->m_n[0]=&m_nodes[idx[0]];
+ pft[0]->m_n[1]=&m_nodes[ni];
+ pft[1]->m_n[0]=&m_nodes[ni];
+ pft[1]->m_n[1]=&m_nodes[idx[1]];
+ }
+ }
+ }
+ /* Refine faces */
+ for(i=0;i<m_faces.size();++i)
+ {
+ const Face& feat=m_faces[i];
+ const int idx[]={ int(feat.m_n[0]-nbase),
+ int(feat.m_n[1]-nbase),
+ int(feat.m_n[2]-nbase)};
+ for(j=2,k=0;k<3;j=k++)
+ {
+ if((idx[j]<ncount)&&(idx[k]<ncount))
+ {
+ const int ni=edges(idx[j],idx[k]);
+ if(ni>0)
+ {
+ appendFace(i);
+ const int l=(k+1)%3;
+ Face* pft[]={ &m_faces[i],
+ &m_faces[m_faces.size()-1]};
+ pft[0]->m_n[0]=&m_nodes[idx[l]];
+ pft[0]->m_n[1]=&m_nodes[idx[j]];
+ pft[0]->m_n[2]=&m_nodes[ni];
+ pft[1]->m_n[0]=&m_nodes[ni];
+ pft[1]->m_n[1]=&m_nodes[idx[k]];
+ pft[1]->m_n[2]=&m_nodes[idx[l]];
+ appendLink(ni,idx[l],pft[0]->m_material);
+ --i;break;
+ }
+ }
+ }
+ }
+ /* Cut */
+ if(cut)
+ {
+ btAlignedObjectArray<int> cnodes;
+ const int pcount=ncount;
+ int i;
+ ncount=m_nodes.size();
+ cnodes.resize(ncount,0);
+ /* Nodes */
+ for(i=0;i<ncount;++i)
+ {
+ const btVector3 x=m_nodes[i].m_x;
+ if((i>=pcount)||(btFabs(ifn->Eval(x))<accurary))
+ {
+ const btVector3 v=m_nodes[i].m_v;
+ btScalar m=getMass(i);
+ if(m>0) { m*=0.5;m_nodes[i].m_im/=0.5; }
+ appendNode(x,m);
+ cnodes[i]=m_nodes.size()-1;
+ m_nodes[cnodes[i]].m_v=v;
+ }
+ }
+ nbase=&m_nodes[0];
+ /* Links */
+ for(i=0,ni=m_links.size();i<ni;++i)
+ {
+ const int id[]={ int(m_links[i].m_n[0]-nbase),
+ int(m_links[i].m_n[1]-nbase)};
+ int todetach=0;
+ if(cnodes[id[0]]&&cnodes[id[1]])
+ {
+ appendLink(i);
+ todetach=m_links.size()-1;
+ }
+ else
+ {
+ if(( (ifn->Eval(m_nodes[id[0]].m_x)<accurary)&&
+ (ifn->Eval(m_nodes[id[1]].m_x)<accurary)))
+ todetach=i;
+ }
+ if(todetach)
+ {
+ Link& l=m_links[todetach];
+ for(int j=0;j<2;++j)
+ {
+ int cn=cnodes[int(l.m_n[j]-nbase)];
+ if(cn) l.m_n[j]=&m_nodes[cn];
+ }
+ }
+ }
+ /* Faces */
+ for(i=0,ni=m_faces.size();i<ni;++i)
+ {
+ Node** n= m_faces[i].m_n;
+ if( (ifn->Eval(n[0]->m_x)<accurary)&&
+ (ifn->Eval(n[1]->m_x)<accurary)&&
+ (ifn->Eval(n[2]->m_x)<accurary))
+ {
+ for(int j=0;j<3;++j)
+ {
+ int cn=cnodes[int(n[j]-nbase)];
+ if(cn) n[j]=&m_nodes[cn];
+ }
+ }
+ }
+ /* Clean orphans */
+ int nnodes=m_nodes.size();
+ btAlignedObjectArray<int> ranks;
+ btAlignedObjectArray<int> todelete;
+ ranks.resize(nnodes,0);
+ for(i=0,ni=m_links.size();i<ni;++i)
+ {
+ for(int j=0;j<2;++j) ranks[int(m_links[i].m_n[j]-nbase)]++;
+ }
+ for(i=0,ni=m_faces.size();i<ni;++i)
+ {
+ for(int j=0;j<3;++j) ranks[int(m_faces[i].m_n[j]-nbase)]++;
+ }
+ for(i=0;i<m_links.size();++i)
+ {
+ const int id[]={ int(m_links[i].m_n[0]-nbase),
+ int(m_links[i].m_n[1]-nbase)};
+ const bool sg[]={ ranks[id[0]]==1,
+ ranks[id[1]]==1};
+ if(sg[0]||sg[1])
+ {
+ --ranks[id[0]];
+ --ranks[id[1]];
+ btSwap(m_links[i],m_links[m_links.size()-1]);
+ m_links.pop_back();--i;
+ }
+ }
+#if 0
+ for(i=nnodes-1;i>=0;--i)
+ {
+ if(!ranks[i]) todelete.push_back(i);
+ }
+ if(todelete.size())
+ {
+ btAlignedObjectArray<int>& map=ranks;
+ for(int i=0;i<nnodes;++i) map[i]=i;
+ PointersToIndices(this);
+ for(int i=0,ni=todelete.size();i<ni;++i)
+ {
+ int j=todelete[i];
+ int& a=map[j];
+ int& b=map[--nnodes];
+ m_ndbvt.remove(m_nodes[a].m_leaf);m_nodes[a].m_leaf=0;
+ btSwap(m_nodes[a],m_nodes[b]);
+ j=a;a=b;b=j;
+ }
+ IndicesToPointers(this,&map[0]);
+ m_nodes.resize(nnodes);
+ }
+#endif
+ }
+ m_bUpdateRtCst=true;
+}
+
+//
+bool btSoftBody::cutLink(const Node* node0,const Node* node1,btScalar position)
+{
+ return(cutLink(int(node0-&m_nodes[0]),int(node1-&m_nodes[0]),position));
+}
+
+//
+bool btSoftBody::cutLink(int node0,int node1,btScalar position)
+{
+ bool done=false;
+ int i,ni;
+ const btVector3 d=m_nodes[node0].m_x-m_nodes[node1].m_x;
+ const btVector3 x=Lerp(m_nodes[node0].m_x,m_nodes[node1].m_x,position);
+ const btVector3 v=Lerp(m_nodes[node0].m_v,m_nodes[node1].m_v,position);
+ const btScalar m=1;
+ appendNode(x,m);
+ appendNode(x,m);
+ Node* pa=&m_nodes[node0];
+ Node* pb=&m_nodes[node1];
+ Node* pn[2]={ &m_nodes[m_nodes.size()-2],
+ &m_nodes[m_nodes.size()-1]};
+ pn[0]->m_v=v;
+ pn[1]->m_v=v;
+ for(i=0,ni=m_links.size();i<ni;++i)
+ {
+ const int mtch=MatchEdge(m_links[i].m_n[0],m_links[i].m_n[1],pa,pb);
+ if(mtch!=-1)
+ {
+ appendLink(i);
+ Link* pft[]={&m_links[i],&m_links[m_links.size()-1]};
+ pft[0]->m_n[1]=pn[mtch];
+ pft[1]->m_n[0]=pn[1-mtch];
+ done=true;
+ }
+ }
+ for(i=0,ni=m_faces.size();i<ni;++i)
+ {
+ for(int k=2,l=0;l<3;k=l++)
+ {
+ const int mtch=MatchEdge(m_faces[i].m_n[k],m_faces[i].m_n[l],pa,pb);
+ if(mtch!=-1)
+ {
+ appendFace(i);
+ Face* pft[]={&m_faces[i],&m_faces[m_faces.size()-1]};
+ pft[0]->m_n[l]=pn[mtch];
+ pft[1]->m_n[k]=pn[1-mtch];
+ appendLink(pn[0],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true);
+ appendLink(pn[1],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true);
+ }
+ }
+ }
+ if(!done)
+ {
+ m_ndbvt.remove(pn[0]->m_leaf);
+ m_ndbvt.remove(pn[1]->m_leaf);
+ m_nodes.pop_back();
+ m_nodes.pop_back();
+ }
+ return(done);
+}
+
+//
+bool btSoftBody::rayTest(const btVector3& rayFrom,
+ const btVector3& rayTo,
+ sRayCast& results)
+{
+ if(m_faces.size()&&m_fdbvt.empty())
+ initializeFaceTree();
+
+ results.body = this;
+ results.fraction = 1.f;
+ results.feature = eFeature::None;
+ results.index = -1;
+
+ return(rayTest(rayFrom,rayTo,results.fraction,results.feature,results.index,false)!=0);
+}
+
+//
+void btSoftBody::setSolver(eSolverPresets::_ preset)
+{
+ m_cfg.m_vsequence.clear();
+ m_cfg.m_psequence.clear();
+ m_cfg.m_dsequence.clear();
+ switch(preset)
+ {
+ case eSolverPresets::Positions:
+ m_cfg.m_psequence.push_back(ePSolver::Anchors);
+ m_cfg.m_psequence.push_back(ePSolver::RContacts);
+ m_cfg.m_psequence.push_back(ePSolver::SContacts);
+ m_cfg.m_psequence.push_back(ePSolver::Linear);
+ break;
+ case eSolverPresets::Velocities:
+ m_cfg.m_vsequence.push_back(eVSolver::Linear);
+
+ m_cfg.m_psequence.push_back(ePSolver::Anchors);
+ m_cfg.m_psequence.push_back(ePSolver::RContacts);
+ m_cfg.m_psequence.push_back(ePSolver::SContacts);
+
+ m_cfg.m_dsequence.push_back(ePSolver::Linear);
+ break;
+ }
+}
+
+//
+void btSoftBody::predictMotion(btScalar dt)
+{
+ int i,ni;
+
+ /* Update */
+ if(m_bUpdateRtCst)
+ {
+ m_bUpdateRtCst=false;
+ updateConstants();
+ m_fdbvt.clear();
+ if(m_cfg.collisions&fCollision::VF_SS)
+ {
+ initializeFaceTree();
+ }
+ }
+
+ /* Prepare */
+ m_sst.sdt = dt*m_cfg.timescale;
+ m_sst.isdt = 1/m_sst.sdt;
+ m_sst.velmrg = m_sst.sdt*3;
+ m_sst.radmrg = getCollisionShape()->getMargin();
+ m_sst.updmrg = m_sst.radmrg*(btScalar)0.25;
+ /* Forces */
+ addVelocity(m_worldInfo->m_gravity*m_sst.sdt);
+ applyForces();
+ /* Integrate */
+ for(i=0,ni=m_nodes.size();i<ni;++i)
+ {
+ Node& n=m_nodes[i];
+ n.m_q = n.m_x;
+ n.m_v += n.m_f*n.m_im*m_sst.sdt;
+ n.m_x += n.m_v*m_sst.sdt;
+ n.m_f = btVector3(0,0,0);
+ }
+ /* Clusters */
+ updateClusters();
+ /* Bounds */
+ updateBounds();
+ /* Nodes */
+ ATTRIBUTE_ALIGNED16(btDbvtVolume) vol;
+ for(i=0,ni=m_nodes.size();i<ni;++i)
+ {
+ Node& n=m_nodes[i];
+ vol = btDbvtVolume::FromCR(n.m_x,m_sst.radmrg);
+ m_ndbvt.update( n.m_leaf,
+ vol,
+ n.m_v*m_sst.velmrg,
+ m_sst.updmrg);
+ }
+ /* Faces */
+ if(!m_fdbvt.empty())
+ {
+ for(int i=0;i<m_faces.size();++i)
+ {
+ Face& f=m_faces[i];
+ const btVector3 v=( f.m_n[0]->m_v+
+ f.m_n[1]->m_v+
+ f.m_n[2]->m_v)/3;
+ vol = VolumeOf(f,m_sst.radmrg);
+ m_fdbvt.update( f.m_leaf,
+ vol,
+ v*m_sst.velmrg,
+ m_sst.updmrg);
+ }
+ }
+ /* Pose */
+ updatePose();
+ /* Match */
+ if(m_pose.m_bframe&&(m_cfg.kMT>0))
+ {
+ const btMatrix3x3 posetrs=m_pose.m_rot;
+ for(int i=0,ni=m_nodes.size();i<ni;++i)
+ {
+ Node& n=m_nodes[i];
+ if(n.m_im>0)
+ {
+ const btVector3 x=posetrs*m_pose.m_pos[i]+m_pose.m_com;
+ n.m_x=Lerp(n.m_x,x,m_cfg.kMT);
+ }
+ }
+ }
+ /* Clear contacts */
+ m_rcontacts.resize(0);
+ m_scontacts.resize(0);
+ /* Optimize dbvt's */
+ m_ndbvt.optimizeIncremental(1);
+ m_fdbvt.optimizeIncremental(1);
+ m_cdbvt.optimizeIncremental(1);
+}
+
+//
+void btSoftBody::solveConstraints()
+{
+ /* Apply clusters */
+ applyClusters(false);
+ /* Prepare links */
+
+ int i,ni;
+
+ for(i=0,ni=m_links.size();i<ni;++i)
+ {
+ Link& l=m_links[i];
+ l.m_c3 = l.m_n[1]->m_q-l.m_n[0]->m_q;
+ l.m_c2 = 1/(l.m_c3.length2()*l.m_c0);
+ }
+ /* Prepare anchors */
+ for(i=0,ni=m_anchors.size();i<ni;++i)
+ {
+ Anchor& a=m_anchors[i];
+ const btVector3 ra=a.m_body->getWorldTransform().getBasis()*a.m_local;
+ a.m_c0 = ImpulseMatrix( m_sst.sdt,
+ a.m_node->m_im,
+ a.m_body->getInvMass(),
+ a.m_body->getInvInertiaTensorWorld(),
+ ra);
+ a.m_c1 = ra;
+ a.m_c2 = m_sst.sdt*a.m_node->m_im;
+ a.m_body->activate();
+ }
+ /* Solve velocities */
+ if(m_cfg.viterations>0)
+ {
+ /* Solve */
+ for(int isolve=0;isolve<m_cfg.viterations;++isolve)
+ {
+ for(int iseq=0;iseq<m_cfg.m_vsequence.size();++iseq)
+ {
+ getSolver(m_cfg.m_vsequence[iseq])(this,1);
+ }
+ }
+ /* Update */
+ for(i=0,ni=m_nodes.size();i<ni;++i)
+ {
+ Node& n=m_nodes[i];
+ n.m_x = n.m_q+n.m_v*m_sst.sdt;
+ }
+ }
+ /* Solve positions */
+ if(m_cfg.piterations>0)
+ {
+ for(int isolve=0;isolve<m_cfg.piterations;++isolve)
+ {
+ const btScalar ti=isolve/(btScalar)m_cfg.piterations;
+ for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq)
+ {
+ getSolver(m_cfg.m_psequence[iseq])(this,1,ti);
+ }
+ }
+ const btScalar vc=m_sst.isdt*(1-m_cfg.kDP);
+ for(i=0,ni=m_nodes.size();i<ni;++i)
+ {
+ Node& n=m_nodes[i];
+ n.m_v = (n.m_x-n.m_q)*vc;
+ n.m_f = btVector3(0,0,0);
+ }
+ }
+ /* Solve drift */
+ if(m_cfg.diterations>0)
+ {
+ const btScalar vcf=m_cfg.kVCF*m_sst.isdt;
+ for(i=0,ni=m_nodes.size();i<ni;++i)
+ {
+ Node& n=m_nodes[i];
+ n.m_q = n.m_x;
+ }
+ for(int idrift=0;idrift<m_cfg.diterations;++idrift)
+ {
+ for(int iseq=0;iseq<m_cfg.m_dsequence.size();++iseq)
+ {
+ getSolver(m_cfg.m_dsequence[iseq])(this,1,0);
+ }
+ }
+ for(int i=0,ni=m_nodes.size();i<ni;++i)
+ {
+ Node& n=m_nodes[i];
+ n.m_v += (n.m_x-n.m_q)*vcf;
+ }
+ }
+ /* Apply clusters */
+ dampClusters();
+ applyClusters(true);
+}
+
+//
+void btSoftBody::staticSolve(int iterations)
+{
+ for(int isolve=0;isolve<iterations;++isolve)
+ {
+ for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq)
+ {
+ getSolver(m_cfg.m_psequence[iseq])(this,1,0);
+ }
+ }
+}
+
+//
+void btSoftBody::solveCommonConstraints(btSoftBody** /*bodies*/,int /*count*/,int /*iterations*/)
+{
+ /// placeholder
+}
+
+//
+void btSoftBody::solveClusters(const btAlignedObjectArray<btSoftBody*>& bodies)
+{
+ const int nb=bodies.size();
+ int iterations=0;
+ int i;
+
+ for(i=0;i<nb;++i)
+ {
+ iterations=btMax(iterations,bodies[i]->m_cfg.citerations);
+ }
+ for(i=0;i<nb;++i)
+ {
+ bodies[i]->prepareClusters(iterations);
+ }
+ for(i=0;i<iterations;++i)
+ {
+ const btScalar sor=1;
+ for(int j=0;j<nb;++j)
+ {
+ bodies[j]->solveClusters(sor);
+ }
+ }
+ for(i=0;i<nb;++i)
+ {
+ bodies[i]->cleanupClusters();
+ }
+}
+
+//
+void btSoftBody::integrateMotion()
+{
+ /* Update */
+ updateNormals();
+}
+
+//
+btSoftBody::RayFromToCaster::RayFromToCaster(const btVector3& rayFrom,const btVector3& rayTo,btScalar mxt)
+{
+ m_rayFrom = rayFrom;
+ m_rayNormalizedDirection = (rayTo-rayFrom);
+ m_rayTo = rayTo;
+ m_mint = mxt;
+ m_face = 0;
+ m_tests = 0;
+}
+
+//
+void btSoftBody::RayFromToCaster::Process(const btDbvtNode* leaf)
+{
+ btSoftBody::Face& f=*(btSoftBody::Face*)leaf->data;
+ const btScalar t=rayFromToTriangle( m_rayFrom,m_rayTo,m_rayNormalizedDirection,
+ f.m_n[0]->m_x,
+ f.m_n[1]->m_x,
+ f.m_n[2]->m_x,
+ m_mint);
+ if((t>0)&&(t<m_mint))
+ {
+ m_mint=t;m_face=&f;
+ }
+ ++m_tests;
+}
+
+//
+btScalar btSoftBody::RayFromToCaster::rayFromToTriangle( const btVector3& rayFrom,
+ const btVector3& rayTo,
+ const btVector3& rayNormalizedDirection,
+ const btVector3& a,
+ const btVector3& b,
+ const btVector3& c,
+ btScalar maxt)
+{
+ static const btScalar ceps=-SIMD_EPSILON*10;
+ static const btScalar teps=SIMD_EPSILON*10;
+
+ const btVector3 n=btCross(b-a,c-a);
+ const btScalar d=btDot(a,n);
+ const btScalar den=btDot(rayNormalizedDirection,n);
+ if(!btFuzzyZero(den))
+ {
+ const btScalar num=btDot(rayFrom,n)-d;
+ const btScalar t=-num/den;
+ if((t>teps)&&(t<maxt))
+ {
+ const btVector3 hit=rayFrom+rayNormalizedDirection*t;
+ if( (btDot(n,btCross(a-hit,b-hit))>ceps) &&
+ (btDot(n,btCross(b-hit,c-hit))>ceps) &&
+ (btDot(n,btCross(c-hit,a-hit))>ceps))
+ {
+ return(t);
+ }
+ }
+ }
+ return(-1);
+}
+
+//
+void btSoftBody::pointersToIndices()
+{
+#define PTR2IDX(_p_,_b_) reinterpret_cast<btSoftBody::Node*>((_p_)-(_b_))
+ btSoftBody::Node* base=&m_nodes[0];
+ int i,ni;
+
+ for(i=0,ni=m_nodes.size();i<ni;++i)
+ {
+ if(m_nodes[i].m_leaf)
+ {
+ m_nodes[i].m_leaf->data=*(void**)&i;
+ }
+ }
+ for(i=0,ni=m_links.size();i<ni;++i)
+ {
+ m_links[i].m_n[0]=PTR2IDX(m_links[i].m_n[0],base);
+ m_links[i].m_n[1]=PTR2IDX(m_links[i].m_n[1],base);
+ }
+ for(i=0,ni=m_faces.size();i<ni;++i)
+ {
+ m_faces[i].m_n[0]=PTR2IDX(m_faces[i].m_n[0],base);
+ m_faces[i].m_n[1]=PTR2IDX(m_faces[i].m_n[1],base);
+ m_faces[i].m_n[2]=PTR2IDX(m_faces[i].m_n[2],base);
+ if(m_faces[i].m_leaf)
+ {
+ m_faces[i].m_leaf->data=*(void**)&i;
+ }
+ }
+ for(i=0,ni=m_anchors.size();i<ni;++i)
+ {
+ m_anchors[i].m_node=PTR2IDX(m_anchors[i].m_node,base);
+ }
+ for(i=0,ni=m_notes.size();i<ni;++i)
+ {
+ for(int j=0;j<m_notes[i].m_rank;++j)
+ {
+ m_notes[i].m_nodes[j]=PTR2IDX(m_notes[i].m_nodes[j],base);
+ }
+ }
+#undef PTR2IDX
+}
+
+//
+void btSoftBody::indicesToPointers(const int* map)
+{
+#define IDX2PTR(_p_,_b_) map?(&(_b_)[map[(((char*)_p_)-(char*)0)]]): \
+ (&(_b_)[(((char*)_p_)-(char*)0)])
+ btSoftBody::Node* base=&m_nodes[0];
+ int i,ni;
+
+ for(i=0,ni=m_nodes.size();i<ni;++i)
+ {
+ if(m_nodes[i].m_leaf)
+ {
+ m_nodes[i].m_leaf->data=&m_nodes[i];
+ }
+ }
+ for(i=0,ni=m_links.size();i<ni;++i)
+ {
+ m_links[i].m_n[0]=IDX2PTR(m_links[i].m_n[0],base);
+ m_links[i].m_n[1]=IDX2PTR(m_links[i].m_n[1],base);
+ }
+ for(i=0,ni=m_faces.size();i<ni;++i)
+ {
+ m_faces[i].m_n[0]=IDX2PTR(m_faces[i].m_n[0],base);
+ m_faces[i].m_n[1]=IDX2PTR(m_faces[i].m_n[1],base);
+ m_faces[i].m_n[2]=IDX2PTR(m_faces[i].m_n[2],base);
+ if(m_faces[i].m_leaf)
+ {
+ m_faces[i].m_leaf->data=&m_faces[i];
+ }
+ }
+ for(i=0,ni=m_anchors.size();i<ni;++i)
+ {
+ m_anchors[i].m_node=IDX2PTR(m_anchors[i].m_node,base);
+ }
+ for(i=0,ni=m_notes.size();i<ni;++i)
+ {
+ for(int j=0;j<m_notes[i].m_rank;++j)
+ {
+ m_notes[i].m_nodes[j]=IDX2PTR(m_notes[i].m_nodes[j],base);
+ }
+ }
+#undef IDX2PTR
+}
+
+//
+int btSoftBody::rayTest(const btVector3& rayFrom,const btVector3& rayTo,
+ btScalar& mint,eFeature::_& feature,int& index,bool bcountonly) const
+{
+ int cnt=0;
+ if(bcountonly||m_fdbvt.empty())
+ {/* Full search */
+ btVector3 dir = rayTo-rayFrom;
+ dir.normalize();
+
+ for(int i=0,ni=m_faces.size();i<ni;++i)
+ {
+ const btSoftBody::Face& f=m_faces[i];
+
+ const btScalar t=RayFromToCaster::rayFromToTriangle( rayFrom,rayTo,dir,
+ f.m_n[0]->m_x,
+ f.m_n[1]->m_x,
+ f.m_n[2]->m_x,
+ mint);
+ if(t>0)
+ {
+ ++cnt;
+ if(!bcountonly)
+ {
+ feature=btSoftBody::eFeature::Face;
+ index=i;
+ mint=t;
+ }
+ }
+ }
+ }
+ else
+ {/* Use dbvt */
+ RayFromToCaster collider(rayFrom,rayTo,mint);
+
+ btDbvt::rayTest(m_fdbvt.m_root,rayFrom,rayTo,collider);
+ if(collider.m_face)
+ {
+ mint=collider.m_mint;
+ feature=btSoftBody::eFeature::Face;
+ index=(int)(collider.m_face-&m_faces[0]);
+ cnt=1;
+ }
+ }
+ return(cnt);
+}
+
+//
+void btSoftBody::initializeFaceTree()
+{
+ m_fdbvt.clear();
+ for(int i=0;i<m_faces.size();++i)
+ {
+ Face& f=m_faces[i];
+ f.m_leaf=m_fdbvt.insert(VolumeOf(f,0),&f);
+ }
+}
+
+//
+btVector3 btSoftBody::evaluateCom() const
+{
+ btVector3 com(0,0,0);
+ if(m_pose.m_bframe)
+ {
+ for(int i=0,ni=m_nodes.size();i<ni;++i)
+ {
+ com+=m_nodes[i].m_x*m_pose.m_wgh[i];
+ }
+ }
+ return(com);
+}
+
+//
+bool btSoftBody::checkContact( btCollisionObject* colObj,
+ const btVector3& x,
+ btScalar margin,
+ btSoftBody::sCti& cti) const
+{
+ btVector3 nrm;
+ btCollisionShape* shp=colObj->getCollisionShape();
+ btRigidBody* tmpRigid = btRigidBody::upcast(colObj);
+ const btTransform& wtr=tmpRigid? tmpRigid->getInterpolationWorldTransform() : colObj->getWorldTransform();
+ btScalar dst=m_worldInfo->m_sparsesdf.Evaluate( wtr.invXform(x),
+ shp,
+ nrm,
+ margin);
+ if(dst<0)
+ {
+ cti.m_colObj = colObj;
+ cti.m_normal = wtr.getBasis()*nrm;
+ cti.m_offset = -btDot( cti.m_normal,
+ x-cti.m_normal*dst);
+ return(true);
+ }
+ return(false);
+}
+
+//
+void btSoftBody::updateNormals()
+{
+ const btVector3 zv(0,0,0);
+ int i,ni;
+
+ for(i=0,ni=m_nodes.size();i<ni;++i)
+ {
+ m_nodes[i].m_n=zv;
+ }
+ for(i=0,ni=m_faces.size();i<ni;++i)
+ {
+ btSoftBody::Face& f=m_faces[i];
+ const btVector3 n=btCross(f.m_n[1]->m_x-f.m_n[0]->m_x,
+ f.m_n[2]->m_x-f.m_n[0]->m_x);
+ f.m_normal=n.normalized();
+ f.m_n[0]->m_n+=n;
+ f.m_n[1]->m_n+=n;
+ f.m_n[2]->m_n+=n;
+ }
+ for(i=0,ni=m_nodes.size();i<ni;++i)
+ {
+ btScalar len = m_nodes[i].m_n.length();
+ if (len>SIMD_EPSILON)
+ m_nodes[i].m_n /= len;
+ }
+}
+
+//
+void btSoftBody::updateBounds()
+{
+ if(m_ndbvt.m_root)
+ {
+ const btVector3& mins=m_ndbvt.m_root->volume.Mins();
+ const btVector3& maxs=m_ndbvt.m_root->volume.Maxs();
+ const btScalar csm=getCollisionShape()->getMargin();
+ const btVector3 mrg=btVector3( csm,
+ csm,
+ csm)*1; // ??? to investigate...
+ m_bounds[0]=mins-mrg;
+ m_bounds[1]=maxs+mrg;
+ if(0!=getBroadphaseHandle())
+ {
+ m_worldInfo->m_broadphase->setAabb( getBroadphaseHandle(),
+ m_bounds[0],
+ m_bounds[1],
+ m_worldInfo->m_dispatcher);
+ }
+ }
+ else
+ {
+ m_bounds[0]=
+ m_bounds[1]=btVector3(0,0,0);
+ }
+}
+
+
+//
+void btSoftBody::updatePose()
+{
+ if(m_pose.m_bframe)
+ {
+ btSoftBody::Pose& pose=m_pose;
+ const btVector3 com=evaluateCom();
+ /* Com */
+ pose.m_com = com;
+ /* Rotation */
+ btMatrix3x3 Apq;
+ const btScalar eps=SIMD_EPSILON;
+ Apq[0]=Apq[1]=Apq[2]=btVector3(0,0,0);
+ Apq[0].setX(eps);Apq[1].setY(eps*2);Apq[2].setZ(eps*3);
+ for(int i=0,ni=m_nodes.size();i<ni;++i)
+ {
+ const btVector3 a=pose.m_wgh[i]*(m_nodes[i].m_x-com);
+ const btVector3& b=pose.m_pos[i];
+ Apq[0]+=a.x()*b;
+ Apq[1]+=a.y()*b;
+ Apq[2]+=a.z()*b;
+ }
+ btMatrix3x3 r,s;
+ PolarDecompose(Apq,r,s);
+ pose.m_rot=r;
+ pose.m_scl=pose.m_aqq*r.transpose()*Apq;
+ if(m_cfg.maxvolume>1)
+ {
+ const btScalar idet=Clamp<btScalar>( 1/pose.m_scl.determinant(),
+ 1,m_cfg.maxvolume);
+ pose.m_scl=Mul(pose.m_scl,idet);
+ }
+
+ }
+}
+
+//
+void btSoftBody::updateConstants()
+{
+ int i,ni;
+
+ /* Links */
+ for(i=0,ni=m_links.size();i<ni;++i)
+ {
+ Link& l=m_links[i];
+ Material& m=*l.m_material;
+ l.m_rl = (l.m_n[0]->m_x-l.m_n[1]->m_x).length();
+ l.m_c0 = (l.m_n[0]->m_im+l.m_n[1]->m_im)/m.m_kLST;
+ l.m_c1 = l.m_rl*l.m_rl;
+ }
+ /* Faces */
+ for(i=0,ni=m_faces.size();i<ni;++i)
+ {
+ Face& f=m_faces[i];
+ f.m_ra = AreaOf(f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x);
+ }
+ /* Area's */
+ btAlignedObjectArray<int> counts;
+ counts.resize(m_nodes.size(),0);
+ for(i=0,ni=m_nodes.size();i<ni;++i)
+ {
+ m_nodes[i].m_area = 0;
+ }
+ for(i=0,ni=m_faces.size();i<ni;++i)
+ {
+ btSoftBody::Face& f=m_faces[i];
+ for(int j=0;j<3;++j)
+ {
+ const int index=(int)(f.m_n[j]-&m_nodes[0]);
+ counts[index]++;
+ f.m_n[j]->m_area+=btFabs(f.m_ra);
+ }
+ }
+ for(i=0,ni=m_nodes.size();i<ni;++i)
+ {
+ if(counts[i]>0)
+ m_nodes[i].m_area/=(btScalar)counts[i];
+ else
+ m_nodes[i].m_area=0;
+ }
+}
+
+//
+void btSoftBody::initializeClusters()
+{
+ int i;
+
+ for( i=0;i<m_clusters.size();++i)
+ {
+ Cluster& c=*m_clusters[i];
+ c.m_imass=0;
+ c.m_masses.resize(c.m_nodes.size());
+ for(int j=0;j<c.m_nodes.size();++j)
+ {
+ if (c.m_nodes[j]->m_im==0)
+ {
+ c.m_containsAnchor = true;
+ c.m_masses[j] = BT_LARGE_FLOAT;
+ } else
+ {
+ c.m_masses[j] = btScalar(1.)/c.m_nodes[j]->m_im;
+ }
+ c.m_imass += c.m_masses[j];
+ }
+ c.m_imass = btScalar(1.)/c.m_imass;
+ c.m_com = btSoftBody::clusterCom(&c);
+ c.m_lv = btVector3(0,0,0);
+ c.m_av = btVector3(0,0,0);
+ c.m_leaf = 0;
+ /* Inertia */
+ btMatrix3x3& ii=c.m_locii;
+ ii[0]=ii[1]=ii[2]=btVector3(0,0,0);
+ {
+ int i,ni;
+
+ for(i=0,ni=c.m_nodes.size();i<ni;++i)
+ {
+ const btVector3 k=c.m_nodes[i]->m_x-c.m_com;
+ const btVector3 q=k*k;
+ const btScalar m=c.m_masses[i];
+ ii[0][0] += m*(q[1]+q[2]);
+ ii[1][1] += m*(q[0]+q[2]);
+ ii[2][2] += m*(q[0]+q[1]);
+ ii[0][1] -= m*k[0]*k[1];
+ ii[0][2] -= m*k[0]*k[2];
+ ii[1][2] -= m*k[1]*k[2];
+ }
+ }
+ ii[1][0]=ii[0][1];
+ ii[2][0]=ii[0][2];
+ ii[2][1]=ii[1][2];
+
+ ii = ii.inverse();
+
+ /* Frame */
+ c.m_framexform.setIdentity();
+ c.m_framexform.setOrigin(c.m_com);
+ c.m_framerefs.resize(c.m_nodes.size());
+ {
+ int i;
+ for(i=0;i<c.m_framerefs.size();++i)
+ {
+ c.m_framerefs[i]=c.m_nodes[i]->m_x-c.m_com;
+ }
+ }
+ }
+}
+
+//
+void btSoftBody::updateClusters()
+{
+ BT_PROFILE("UpdateClusters");
+ int i;
+
+ for(i=0;i<m_clusters.size();++i)
+ {
+ btSoftBody::Cluster& c=*m_clusters[i];
+ const int n=c.m_nodes.size();
+ //const btScalar invn=1/(btScalar)n;
+ if(n)
+ {
+ /* Frame */
+ const btScalar eps=btScalar(0.0001);
+ btMatrix3x3 m,r,s;
+ m[0]=m[1]=m[2]=btVector3(0,0,0);
+ m[0][0]=eps*1;
+ m[1][1]=eps*2;
+ m[2][2]=eps*3;
+ c.m_com=clusterCom(&c);
+ for(int i=0;i<c.m_nodes.size();++i)
+ {
+ const btVector3 a=c.m_nodes[i]->m_x-c.m_com;
+ const btVector3& b=c.m_framerefs[i];
+ m[0]+=a[0]*b;m[1]+=a[1]*b;m[2]+=a[2]*b;
+ }
+ PolarDecompose(m,r,s);
+ c.m_framexform.setOrigin(c.m_com);
+ c.m_framexform.setBasis(r);
+ /* Inertia */
+#if 1/* Constant */
+ c.m_invwi=c.m_framexform.getBasis()*c.m_locii*c.m_framexform.getBasis().transpose();
+#else
+#if 0/* Sphere */
+ const btScalar rk=(2*c.m_extents.length2())/(5*c.m_imass);
+ const btVector3 inertia(rk,rk,rk);
+ const btVector3 iin(btFabs(inertia[0])>SIMD_EPSILON?1/inertia[0]:0,
+ btFabs(inertia[1])>SIMD_EPSILON?1/inertia[1]:0,
+ btFabs(inertia[2])>SIMD_EPSILON?1/inertia[2]:0);
+
+ c.m_invwi=c.m_xform.getBasis().scaled(iin)*c.m_xform.getBasis().transpose();
+#else/* Actual */
+ c.m_invwi[0]=c.m_invwi[1]=c.m_invwi[2]=btVector3(0,0,0);
+ for(int i=0;i<n;++i)
+ {
+ const btVector3 k=c.m_nodes[i]->m_x-c.m_com;
+ const btVector3 q=k*k;
+ const btScalar m=1/c.m_nodes[i]->m_im;
+ c.m_invwi[0][0] += m*(q[1]+q[2]);
+ c.m_invwi[1][1] += m*(q[0]+q[2]);
+ c.m_invwi[2][2] += m*(q[0]+q[1]);
+ c.m_invwi[0][1] -= m*k[0]*k[1];
+ c.m_invwi[0][2] -= m*k[0]*k[2];
+ c.m_invwi[1][2] -= m*k[1]*k[2];
+ }
+ c.m_invwi[1][0]=c.m_invwi[0][1];
+ c.m_invwi[2][0]=c.m_invwi[0][2];
+ c.m_invwi[2][1]=c.m_invwi[1][2];
+ c.m_invwi=c.m_invwi.inverse();
+#endif
+#endif
+ /* Velocities */
+ c.m_lv=btVector3(0,0,0);
+ c.m_av=btVector3(0,0,0);
+ {
+ int i;
+
+ for(i=0;i<n;++i)
+ {
+ const btVector3 v=c.m_nodes[i]->m_v*c.m_masses[i];
+ c.m_lv += v;
+ c.m_av += btCross(c.m_nodes[i]->m_x-c.m_com,v);
+ }
+ }
+ c.m_lv=c.m_imass*c.m_lv*(1-c.m_ldamping);
+ c.m_av=c.m_invwi*c.m_av*(1-c.m_adamping);
+ c.m_vimpulses[0] =
+ c.m_vimpulses[1] = btVector3(0,0,0);
+ c.m_dimpulses[0] =
+ c.m_dimpulses[1] = btVector3(0,0,0);
+ c.m_nvimpulses = 0;
+ c.m_ndimpulses = 0;
+ /* Matching */
+ if(c.m_matching>0)
+ {
+ for(int j=0;j<c.m_nodes.size();++j)
+ {
+ Node& n=*c.m_nodes[j];
+ const btVector3 x=c.m_framexform*c.m_framerefs[j];
+ n.m_x=Lerp(n.m_x,x,c.m_matching);
+ }
+ }
+ /* Dbvt */
+ if(c.m_collide)
+ {
+ btVector3 mi=c.m_nodes[0]->m_x;
+ btVector3 mx=mi;
+ for(int j=1;j<n;++j)
+ {
+ mi.setMin(c.m_nodes[j]->m_x);
+ mx.setMax(c.m_nodes[j]->m_x);
+ }
+ ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(mi,mx);
+ if(c.m_leaf)
+ m_cdbvt.update(c.m_leaf,bounds,c.m_lv*m_sst.sdt*3,m_sst.radmrg);
+ else
+ c.m_leaf=m_cdbvt.insert(bounds,&c);
+ }
+ }
+ }
+
+
+}
+
+
+
+
+//
+void btSoftBody::cleanupClusters()
+{
+ for(int i=0;i<m_joints.size();++i)
+ {
+ m_joints[i]->Terminate(m_sst.sdt);
+ if(m_joints[i]->m_delete)
+ {
+ btAlignedFree(m_joints[i]);
+ m_joints.remove(m_joints[i--]);
+ }
+ }
+}
+
+//
+void btSoftBody::prepareClusters(int iterations)
+{
+ for(int i=0;i<m_joints.size();++i)
+ {
+ m_joints[i]->Prepare(m_sst.sdt,iterations);
+ }
+}
+
+
+//
+void btSoftBody::solveClusters(btScalar sor)
+{
+ for(int i=0,ni=m_joints.size();i<ni;++i)
+ {
+ m_joints[i]->Solve(m_sst.sdt,sor);
+ }
+}
+
+//
+void btSoftBody::applyClusters(bool drift)
+{
+ BT_PROFILE("ApplyClusters");
+ const btScalar f0=m_sst.sdt;
+ //const btScalar f1=f0/2;
+ btAlignedObjectArray<btVector3> deltas;
+ btAlignedObjectArray<btScalar> weights;
+ deltas.resize(m_nodes.size(),btVector3(0,0,0));
+ weights.resize(m_nodes.size(),0);
+ int i;
+
+ if(drift)
+ {
+ for(i=0;i<m_clusters.size();++i)
+ {
+ Cluster& c=*m_clusters[i];
+ if(c.m_ndimpulses)
+ {
+ c.m_dimpulses[0]/=(btScalar)c.m_ndimpulses;
+ c.m_dimpulses[1]/=(btScalar)c.m_ndimpulses;
+ }
+ }
+ }
+
+ for(i=0;i<m_clusters.size();++i)
+ {
+ Cluster& c=*m_clusters[i];
+ if(0<(drift?c.m_ndimpulses:c.m_nvimpulses))
+ {
+ const btVector3 v=(drift?c.m_dimpulses[0]:c.m_vimpulses[0])*m_sst.sdt;
+ const btVector3 w=(drift?c.m_dimpulses[1]:c.m_vimpulses[1])*m_sst.sdt;
+ for(int j=0;j<c.m_nodes.size();++j)
+ {
+ const int idx=int(c.m_nodes[j]-&m_nodes[0]);
+ const btVector3& x=c.m_nodes[j]->m_x;
+ const btScalar q=c.m_masses[j];
+ deltas[idx] += (v+btCross(w,x-c.m_com))*q;
+ weights[idx] += q;
+ }
+ }
+ }
+ for(i=0;i<deltas.size();++i)
+ {
+ if(weights[i]>0) m_nodes[i].m_x+=deltas[i]/weights[i];
+ }
+}
+
+//
+void btSoftBody::dampClusters()
+{
+ int i;
+
+ for(i=0;i<m_clusters.size();++i)
+ {
+ Cluster& c=*m_clusters[i];
+ if(c.m_ndamping>0)
+ {
+ for(int j=0;j<c.m_nodes.size();++j)
+ {
+ Node& n=*c.m_nodes[j];
+ if(n.m_im>0)
+ {
+ const btVector3 vx=c.m_lv+btCross(c.m_av,c.m_nodes[j]->m_q-c.m_com);
+ if(vx.length2()<=n.m_v.length2())
+ {
+ n.m_v += c.m_ndamping*(vx-n.m_v);
+ }
+ }
+ }
+ }
+ }
+}
+
+//
+void btSoftBody::Joint::Prepare(btScalar dt,int)
+{
+ m_bodies[0].activate();
+ m_bodies[1].activate();
+}
+
+//
+void btSoftBody::LJoint::Prepare(btScalar dt,int iterations)
+{
+ static const btScalar maxdrift=4;
+ Joint::Prepare(dt,iterations);
+ m_rpos[0] = m_bodies[0].xform()*m_refs[0];
+ m_rpos[1] = m_bodies[1].xform()*m_refs[1];
+ m_drift = Clamp(m_rpos[0]-m_rpos[1],maxdrift)*m_erp/dt;
+ m_rpos[0] -= m_bodies[0].xform().getOrigin();
+ m_rpos[1] -= m_bodies[1].xform().getOrigin();
+ m_massmatrix = ImpulseMatrix( m_bodies[0].invMass(),m_bodies[0].invWorldInertia(),m_rpos[0],
+ m_bodies[1].invMass(),m_bodies[1].invWorldInertia(),m_rpos[1]);
+ if(m_split>0)
+ {
+ m_sdrift = m_massmatrix*(m_drift*m_split);
+ m_drift *= 1-m_split;
+ }
+ m_drift /=(btScalar)iterations;
+}
+
+//
+void btSoftBody::LJoint::Solve(btScalar dt,btScalar sor)
+{
+ const btVector3 va=m_bodies[0].velocity(m_rpos[0]);
+ const btVector3 vb=m_bodies[1].velocity(m_rpos[1]);
+ const btVector3 vr=va-vb;
+ btSoftBody::Impulse impulse;
+ impulse.m_asVelocity = 1;
+ impulse.m_velocity = m_massmatrix*(m_drift+vr*m_cfm)*sor;
+ m_bodies[0].applyImpulse(-impulse,m_rpos[0]);
+ m_bodies[1].applyImpulse( impulse,m_rpos[1]);
+}
+
+//
+void btSoftBody::LJoint::Terminate(btScalar dt)
+{
+ if(m_split>0)
+ {
+ m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]);
+ m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]);
+ }
+}
+
+//
+void btSoftBody::AJoint::Prepare(btScalar dt,int iterations)
+{
+ static const btScalar maxdrift=SIMD_PI/16;
+ m_icontrol->Prepare(this);
+ Joint::Prepare(dt,iterations);
+ m_axis[0] = m_bodies[0].xform().getBasis()*m_refs[0];
+ m_axis[1] = m_bodies[1].xform().getBasis()*m_refs[1];
+ m_drift = NormalizeAny(btCross(m_axis[1],m_axis[0]));
+ m_drift *= btMin(maxdrift,btAcos(Clamp<btScalar>(btDot(m_axis[0],m_axis[1]),-1,+1)));
+ m_drift *= m_erp/dt;
+ m_massmatrix= AngularImpulseMatrix(m_bodies[0].invWorldInertia(),m_bodies[1].invWorldInertia());
+ if(m_split>0)
+ {
+ m_sdrift = m_massmatrix*(m_drift*m_split);
+ m_drift *= 1-m_split;
+ }
+ m_drift /=(btScalar)iterations;
+}
+
+//
+void btSoftBody::AJoint::Solve(btScalar dt,btScalar sor)
+{
+ const btVector3 va=m_bodies[0].angularVelocity();
+ const btVector3 vb=m_bodies[1].angularVelocity();
+ const btVector3 vr=va-vb;
+ const btScalar sp=btDot(vr,m_axis[0]);
+ const btVector3 vc=vr-m_axis[0]*m_icontrol->Speed(this,sp);
+ btSoftBody::Impulse impulse;
+ impulse.m_asVelocity = 1;
+ impulse.m_velocity = m_massmatrix*(m_drift+vc*m_cfm)*sor;
+ m_bodies[0].applyAImpulse(-impulse);
+ m_bodies[1].applyAImpulse( impulse);
+}
+
+//
+void btSoftBody::AJoint::Terminate(btScalar dt)
+{
+ if(m_split>0)
+ {
+ m_bodies[0].applyDAImpulse(-m_sdrift);
+ m_bodies[1].applyDAImpulse( m_sdrift);
+ }
+}
+
+//
+void btSoftBody::CJoint::Prepare(btScalar dt,int iterations)
+{
+ Joint::Prepare(dt,iterations);
+ const bool dodrift=(m_life==0);
+ m_delete=(++m_life)>m_maxlife;
+ if(dodrift)
+ {
+ m_drift=m_drift*m_erp/dt;
+ if(m_split>0)
+ {
+ m_sdrift = m_massmatrix*(m_drift*m_split);
+ m_drift *= 1-m_split;
+ }
+ m_drift/=(btScalar)iterations;
+ }
+ else
+ {
+ m_drift=m_sdrift=btVector3(0,0,0);
+ }
+}
+
+//
+void btSoftBody::CJoint::Solve(btScalar dt,btScalar sor)
+{
+ const btVector3 va=m_bodies[0].velocity(m_rpos[0]);
+ const btVector3 vb=m_bodies[1].velocity(m_rpos[1]);
+ const btVector3 vrel=va-vb;
+ const btScalar rvac=btDot(vrel,m_normal);
+ btSoftBody::Impulse impulse;
+ impulse.m_asVelocity = 1;
+ impulse.m_velocity = m_drift;
+ if(rvac<0)
+ {
+ const btVector3 iv=m_normal*rvac;
+ const btVector3 fv=vrel-iv;
+ impulse.m_velocity += iv+fv*m_friction;
+ }
+ impulse.m_velocity=m_massmatrix*impulse.m_velocity*sor;
+
+ if (m_bodies[0].m_soft==m_bodies[1].m_soft)
+ {
+ if ((impulse.m_velocity.getX() ==impulse.m_velocity.getX())&&(impulse.m_velocity.getY() ==impulse.m_velocity.getY())&&
+ (impulse.m_velocity.getZ() ==impulse.m_velocity.getZ()))
+ {
+ if (impulse.m_asVelocity)
+ {
+ if (impulse.m_velocity.length() <m_bodies[0].m_soft->m_maxSelfCollisionImpulse)
+ {
+
+ } else
+ {
+ m_bodies[0].applyImpulse(-impulse*m_bodies[0].m_soft->m_selfCollisionImpulseFactor,m_rpos[0]);
+ m_bodies[1].applyImpulse( impulse*m_bodies[0].m_soft->m_selfCollisionImpulseFactor,m_rpos[1]);
+ }
+ }
+ }
+ } else
+ {
+ m_bodies[0].applyImpulse(-impulse,m_rpos[0]);
+ m_bodies[1].applyImpulse( impulse,m_rpos[1]);
+ }
+}
+
+//
+void btSoftBody::CJoint::Terminate(btScalar dt)
+{
+ if(m_split>0)
+ {
+ m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]);
+ m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]);
+ }
+}
+
+//
+void btSoftBody::applyForces()
+{
+
+ BT_PROFILE("SoftBody applyForces");
+ const btScalar dt=m_sst.sdt;
+ const btScalar kLF=m_cfg.kLF;
+ const btScalar kDG=m_cfg.kDG;
+ const btScalar kPR=m_cfg.kPR;
+ const btScalar kVC=m_cfg.kVC;
+ const bool as_lift=kLF>0;
+ const bool as_drag=kDG>0;
+ const bool as_pressure=kPR!=0;
+ const bool as_volume=kVC>0;
+ const bool as_aero= as_lift ||
+ as_drag ;
+ const bool as_vaero= as_aero &&
+ (m_cfg.aeromodel<btSoftBody::eAeroModel::F_TwoSided);
+ const bool as_faero= as_aero &&
+ (m_cfg.aeromodel>=btSoftBody::eAeroModel::F_TwoSided);
+ const bool use_medium= as_aero;
+ const bool use_volume= as_pressure ||
+ as_volume ;
+ btScalar volume=0;
+ btScalar ivolumetp=0;
+ btScalar dvolumetv=0;
+ btSoftBody::sMedium medium;
+ if(use_volume)
+ {
+ volume = getVolume();
+ ivolumetp = 1/btFabs(volume)*kPR;
+ dvolumetv = (m_pose.m_volume-volume)*kVC;
+ }
+ /* Per vertex forces */
+ int i,ni;
+
+ for(i=0,ni=m_nodes.size();i<ni;++i)
+ {
+ btSoftBody::Node& n=m_nodes[i];
+ if(n.m_im>0)
+ {
+ if(use_medium)
+ {
+ EvaluateMedium(m_worldInfo,n.m_x,medium);
+ /* Aerodynamics */
+ if(as_vaero)
+ {
+ const btVector3 rel_v=n.m_v-medium.m_velocity;
+ const btScalar rel_v2=rel_v.length2();
+ if(rel_v2>SIMD_EPSILON)
+ {
+ btVector3 nrm=n.m_n;
+ /* Setup normal */
+ switch(m_cfg.aeromodel)
+ {
+ case btSoftBody::eAeroModel::V_Point:
+ nrm=NormalizeAny(rel_v);break;
+ case btSoftBody::eAeroModel::V_TwoSided:
+ nrm*=(btScalar)(btDot(nrm,rel_v)<0?-1:+1);break;
+ }
+ const btScalar dvn=btDot(rel_v,nrm);
+ /* Compute forces */
+ if(dvn>0)
+ {
+ btVector3 force(0,0,0);
+ const btScalar c0 = n.m_area*dvn*rel_v2/2;
+ const btScalar c1 = c0*medium.m_density;
+ force += nrm*(-c1*kLF);
+ force += rel_v.normalized()*(-c1*kDG);
+ ApplyClampedForce(n,force,dt);
+ }
+ }
+ }
+ }
+ /* Pressure */
+ if(as_pressure)
+ {
+ n.m_f += n.m_n*(n.m_area*ivolumetp);
+ }
+ /* Volume */
+ if(as_volume)
+ {
+ n.m_f += n.m_n*(n.m_area*dvolumetv);
+ }
+ }
+ }
+ /* Per face forces */
+ for(i=0,ni=m_faces.size();i<ni;++i)
+ {
+ btSoftBody::Face& f=m_faces[i];
+ if(as_faero)
+ {
+ const btVector3 v=(f.m_n[0]->m_v+f.m_n[1]->m_v+f.m_n[2]->m_v)/3;
+ const btVector3 x=(f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3;
+ EvaluateMedium(m_worldInfo,x,medium);
+ const btVector3 rel_v=v-medium.m_velocity;
+ const btScalar rel_v2=rel_v.length2();
+ if(rel_v2>SIMD_EPSILON)
+ {
+ btVector3 nrm=f.m_normal;
+ /* Setup normal */
+ switch(m_cfg.aeromodel)
+ {
+ case btSoftBody::eAeroModel::F_TwoSided:
+ nrm*=(btScalar)(btDot(nrm,rel_v)<0?-1:+1);break;
+ }
+ const btScalar dvn=btDot(rel_v,nrm);
+ /* Compute forces */
+ if(dvn>0)
+ {
+ btVector3 force(0,0,0);
+ const btScalar c0 = f.m_ra*dvn*rel_v2;
+ const btScalar c1 = c0*medium.m_density;
+ force += nrm*(-c1*kLF);
+ force += rel_v.normalized()*(-c1*kDG);
+ force /= 3;
+ for(int j=0;j<3;++j) ApplyClampedForce(*f.m_n[j],force,dt);
+ }
+ }
+ }
+ }
+}
+
+//
+void btSoftBody::PSolve_Anchors(btSoftBody* psb,btScalar kst,btScalar ti)
+{
+ const btScalar kAHR=psb->m_cfg.kAHR*kst;
+ const btScalar dt=psb->m_sst.sdt;
+ for(int i=0,ni=psb->m_anchors.size();i<ni;++i)
+ {
+ const Anchor& a=psb->m_anchors[i];
+ const btTransform& t=a.m_body->getInterpolationWorldTransform();
+ Node& n=*a.m_node;
+ const btVector3 wa=t*a.m_local;
+ const btVector3 va=a.m_body->getVelocityInLocalPoint(a.m_c1)*dt;
+ const btVector3 vb=n.m_x-n.m_q;
+ const btVector3 vr=(va-vb)+(wa-n.m_x)*kAHR;
+ const btVector3 impulse=a.m_c0*vr;
+ n.m_x+=impulse*a.m_c2;
+ a.m_body->applyImpulse(-impulse,a.m_c1);
+ }
+}
+
+//
+void btSoftBody::PSolve_RContacts(btSoftBody* psb,btScalar kst,btScalar ti)
+{
+ const btScalar dt=psb->m_sst.sdt;
+ const btScalar mrg=psb->getCollisionShape()->getMargin();
+ for(int i=0,ni=psb->m_rcontacts.size();i<ni;++i)
+ {
+ const RContact& c=psb->m_rcontacts[i];
+ const sCti& cti=c.m_cti;
+ btRigidBody* tmpRigid = btRigidBody::upcast(cti.m_colObj);
+
+ const btVector3 va=tmpRigid ? tmpRigid->getVelocityInLocalPoint(c.m_c1)*dt : btVector3(0,0,0);
+ const btVector3 vb=c.m_node->m_x-c.m_node->m_q;
+ const btVector3 vr=vb-va;
+ const btScalar dn=btDot(vr,cti.m_normal);
+ if(dn<=SIMD_EPSILON)
+ {
+ const btScalar dp=btMin(btDot(c.m_node->m_x,cti.m_normal)+cti.m_offset,mrg);
+ const btVector3 fv=vr-cti.m_normal*dn;
+ const btVector3 impulse=c.m_c0*((vr-fv*c.m_c3+cti.m_normal*(dp*c.m_c4))*kst);
+ c.m_node->m_x-=impulse*c.m_c2;
+ if (tmpRigid)
+ tmpRigid->applyImpulse(impulse,c.m_c1);
+ }
+ }
+}
+
+//
+void btSoftBody::PSolve_SContacts(btSoftBody* psb,btScalar,btScalar ti)
+{
+ for(int i=0,ni=psb->m_scontacts.size();i<ni;++i)
+ {
+ const SContact& c=psb->m_scontacts[i];
+ const btVector3& nr=c.m_normal;
+ Node& n=*c.m_node;
+ Face& f=*c.m_face;
+ const btVector3 p=BaryEval( f.m_n[0]->m_x,
+ f.m_n[1]->m_x,
+ f.m_n[2]->m_x,
+ c.m_weights);
+ const btVector3 q=BaryEval( f.m_n[0]->m_q,
+ f.m_n[1]->m_q,
+ f.m_n[2]->m_q,
+ c.m_weights);
+ const btVector3 vr=(n.m_x-n.m_q)-(p-q);
+ btVector3 corr(0,0,0);
+ btScalar dot = btDot(vr,nr);
+ if(dot<0)
+ {
+ const btScalar j=c.m_margin-(btDot(nr,n.m_x)-btDot(nr,p));
+ corr+=c.m_normal*j;
+ }
+ corr -= ProjectOnPlane(vr,nr)*c.m_friction;
+ n.m_x += corr*c.m_cfm[0];
+ f.m_n[0]->m_x -= corr*(c.m_cfm[1]*c.m_weights.x());
+ f.m_n[1]->m_x -= corr*(c.m_cfm[1]*c.m_weights.y());
+ f.m_n[2]->m_x -= corr*(c.m_cfm[1]*c.m_weights.z());
+ }
+}
+
+//
+void btSoftBody::PSolve_Links(btSoftBody* psb,btScalar kst,btScalar ti)
+{
+ for(int i=0,ni=psb->m_links.size();i<ni;++i)
+ {
+ Link& l=psb->m_links[i];
+ if(l.m_c0>0)
+ {
+ Node& a=*l.m_n[0];
+ Node& b=*l.m_n[1];
+ const btVector3 del=b.m_x-a.m_x;
+ const btScalar len=del.length2();
+ const btScalar k=((l.m_c1-len)/(l.m_c0*(l.m_c1+len)))*kst;
+ //const btScalar t=k*a.m_im;
+ a.m_x-=del*(k*a.m_im);
+ b.m_x+=del*(k*b.m_im);
+ }
+ }
+}
+
+//
+void btSoftBody::VSolve_Links(btSoftBody* psb,btScalar kst)
+{
+ for(int i=0,ni=psb->m_links.size();i<ni;++i)
+ {
+ Link& l=psb->m_links[i];
+ Node** n=l.m_n;
+ const btScalar j=-btDot(l.m_c3,n[0]->m_v-n[1]->m_v)*l.m_c2*kst;
+ n[0]->m_v+= l.m_c3*(j*n[0]->m_im);
+ n[1]->m_v-= l.m_c3*(j*n[1]->m_im);
+ }
+}
+
+//
+btSoftBody::psolver_t btSoftBody::getSolver(ePSolver::_ solver)
+{
+ switch(solver)
+ {
+ case ePSolver::Anchors:
+ return(&btSoftBody::PSolve_Anchors);
+ case ePSolver::Linear:
+ return(&btSoftBody::PSolve_Links);
+ case ePSolver::RContacts:
+ return(&btSoftBody::PSolve_RContacts);
+ case ePSolver::SContacts:
+ return(&btSoftBody::PSolve_SContacts);
+ }
+ return(0);
+}
+
+//
+btSoftBody::vsolver_t btSoftBody::getSolver(eVSolver::_ solver)
+{
+ switch(solver)
+ {
+ case eVSolver::Linear: return(&btSoftBody::VSolve_Links);
+ }
+ return(0);
+}
+
+//
+void btSoftBody::defaultCollisionHandler(btCollisionObject* pco)
+{
+ switch(m_cfg.collisions&fCollision::RVSmask)
+ {
+ case fCollision::SDF_RS:
+ {
+ btSoftColliders::CollideSDF_RS docollide;
+ btRigidBody* prb1=btRigidBody::upcast(pco);
+ btTransform wtr=prb1 ? prb1->getInterpolationWorldTransform() : pco->getWorldTransform();
+
+ const btTransform ctr=pco->getWorldTransform();
+ const btScalar timemargin=(wtr.getOrigin()-ctr.getOrigin()).length();
+ const btScalar basemargin=getCollisionShape()->getMargin();
+ btVector3 mins;
+ btVector3 maxs;
+ ATTRIBUTE_ALIGNED16(btDbvtVolume) volume;
+ pco->getCollisionShape()->getAabb( pco->getInterpolationWorldTransform(),
+ mins,
+ maxs);
+ volume=btDbvtVolume::FromMM(mins,maxs);
+ volume.Expand(btVector3(basemargin,basemargin,basemargin));
+ docollide.psb = this;
+ docollide.m_colObj1 = pco;
+ docollide.m_rigidBody = prb1;
+
+ docollide.dynmargin = basemargin+timemargin;
+ docollide.stamargin = basemargin;
+ m_ndbvt.collideTV(m_ndbvt.m_root,volume,docollide);
+ }
+ break;
+ case fCollision::CL_RS:
+ {
+ btSoftColliders::CollideCL_RS collider;
+ collider.Process(this,pco);
+ }
+ break;
+ }
+}
+
+//
+void btSoftBody::defaultCollisionHandler(btSoftBody* psb)
+{
+ const int cf=m_cfg.collisions&psb->m_cfg.collisions;
+ switch(cf&fCollision::SVSmask)
+ {
+ case fCollision::CL_SS:
+ {
+
+ //support self-collision if CL_SELF flag set
+ if (this!=psb || psb->m_cfg.collisions&fCollision::CL_SELF)
+ {
+ btSoftColliders::CollideCL_SS docollide;
+ docollide.Process(this,psb);
+ }
+
+ }
+ break;
+ case fCollision::VF_SS:
+ {
+ //only self-collision for Cluster, not Vertex-Face yet
+ if (this!=psb)
+ {
+ btSoftColliders::CollideVF_SS docollide;
+ /* common */
+ docollide.mrg= getCollisionShape()->getMargin()+
+ psb->getCollisionShape()->getMargin();
+ /* psb0 nodes vs psb1 faces */
+ docollide.psb[0]=this;
+ docollide.psb[1]=psb;
+ docollide.psb[0]->m_ndbvt.collideTT( docollide.psb[0]->m_ndbvt.m_root,
+ docollide.psb[1]->m_fdbvt.m_root,
+ docollide);
+ /* psb1 nodes vs psb0 faces */
+ docollide.psb[0]=psb;
+ docollide.psb[1]=this;
+ docollide.psb[0]->m_ndbvt.collideTT( docollide.psb[0]->m_ndbvt.m_root,
+ docollide.psb[1]->m_fdbvt.m_root,
+ docollide);
+ }
+ }
+ break;
+ default:
+ {
+
+ }
+ }
+}
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btSoftBodyConcaveCollisionAlgorithm.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btSoftBodyConcaveCollisionAlgorithm.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btSoftBodyConcaveCollisionAlgorithm.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btSoftBodyConcaveCollisionAlgorithm.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,368 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/CollisionShapes/btConcaveShape.h"
+#include "BulletCollision/CollisionDispatch/btManifoldResult.h"
+#include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h"
+#include "BulletCollision/CollisionShapes/btTriangleShape.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+#include "BulletCollision/CollisionShapes/btTetrahedronShape.h"
+#include "BulletCollision/CollisionShapes/btConvexHullShape.h"
+
+
+
+#include "LinearMath/btIDebugDraw.h"
+#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
+#include "BulletSoftBody/btSoftBody.h"
+
+#define BT_SOFTBODY_TRIANGLE_EXTRUSION btScalar(0.06)//make this configurable
+
+btSoftBodyConcaveCollisionAlgorithm::btSoftBodyConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1,bool isSwapped)
+: btCollisionAlgorithm(ci),
+m_isSwapped(isSwapped),
+m_btSoftBodyTriangleCallback(ci.m_dispatcher1,body0,body1,isSwapped)
+{
+}
+
+
+
+btSoftBodyConcaveCollisionAlgorithm::~btSoftBodyConcaveCollisionAlgorithm()
+{
+}
+
+
+
+btSoftBodyTriangleCallback::btSoftBodyTriangleCallback(btDispatcher* dispatcher,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped):
+m_dispatcher(dispatcher),
+m_dispatchInfoPtr(0)
+{
+ m_softBody = (btSoftBody*) (isSwapped? body1:body0);
+ m_triBody = isSwapped? body0:body1;
+
+ //
+ // create the manifold from the dispatcher 'manifold pool'
+ //
+ // m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBody,m_triBody);
+
+ clearCache();
+}
+
+btSoftBodyTriangleCallback::~btSoftBodyTriangleCallback()
+{
+ clearCache();
+ // m_dispatcher->releaseManifold( m_manifoldPtr );
+
+}
+
+
+void btSoftBodyTriangleCallback::clearCache()
+{
+ for (int i=0;i<m_shapeCache.size();i++)
+ {
+ btTriIndex* tmp = m_shapeCache.getAtIndex(i);
+ btAssert(tmp);
+ btAssert(tmp->m_childShape);
+ m_softBody->getWorldInfo()->m_sparsesdf.RemoveReferences(tmp->m_childShape);//necessary?
+ delete tmp->m_childShape;
+ }
+ m_shapeCache.clear();
+}
+
+
+void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex)
+{
+ //just for debugging purposes
+ //printf("triangle %d",m_triangleCount++);
+ btCollisionObject* ob = static_cast<btCollisionObject*>(m_triBody);
+ btCollisionAlgorithmConstructionInfo ci;
+ ci.m_dispatcher1 = m_dispatcher;
+
+ ///debug drawing of the overlapping triangles
+ if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && m_dispatchInfoPtr->m_debugDraw->getDebugMode() &btIDebugDraw::DBG_DrawWireframe)
+ {
+ btVector3 color(255,255,0);
+ btTransform& tr = ob->getWorldTransform();
+ m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(triangle[1]),color);
+ m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(triangle[2]),color);
+ m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(triangle[0]),color);
+ }
+
+ btTriIndex triIndex(partId,triangleIndex,0);
+ btHashKey<btTriIndex> triKey(triIndex.getUid());
+
+
+ btTriIndex* shapeIndex = m_shapeCache[triKey];
+ if (shapeIndex)
+ {
+ btCollisionShape* tm = shapeIndex->m_childShape;
+ btAssert(tm);
+
+ //copy over user pointers to temporary shape
+ tm->setUserPointer(ob->getRootCollisionShape()->getUserPointer());
+
+ btCollisionShape* tmpShape = ob->getCollisionShape();
+ ob->internalSetTemporaryCollisionShape( tm );
+
+
+ btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_softBody,m_triBody,0);//m_manifoldPtr);
+
+ colAlgo->processCollision(m_softBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
+ colAlgo->~btCollisionAlgorithm();
+ ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo);
+ ob->internalSetTemporaryCollisionShape( tmpShape);
+ return;
+ }
+
+ //aabb filter is already applied!
+
+ //btCollisionObject* colObj = static_cast<btCollisionObject*>(m_convexProxy->m_clientObject);
+
+ // if (m_softBody->getCollisionShape()->getShapeType()==
+ {
+ // btVector3 other;
+ btVector3 normal = (triangle[1]-triangle[0]).cross(triangle[2]-triangle[0]);
+ normal.normalize();
+ normal*= BT_SOFTBODY_TRIANGLE_EXTRUSION;
+ // other=(triangle[0]+triangle[1]+triangle[2])*0.333333f;
+ // other+=normal*22.f;
+ btVector3 pts[6] = {triangle[0]+normal,
+ triangle[1]+normal,
+ triangle[2]+normal,
+ triangle[0]-normal,
+ triangle[1]-normal,
+ triangle[2]-normal};
+
+ btConvexHullShape* tm = new btConvexHullShape(&pts[0].getX(),6);
+
+
+ // btBU_Simplex1to4 tm(triangle[0],triangle[1],triangle[2],other);
+
+ //btTriangleShape tm(triangle[0],triangle[1],triangle[2]);
+ // tm.setMargin(m_collisionMarginTriangle);
+
+ //copy over user pointers to temporary shape
+ tm->setUserPointer(ob->getRootCollisionShape()->getUserPointer());
+
+ btCollisionShape* tmpShape = ob->getCollisionShape();
+ ob->internalSetTemporaryCollisionShape( tm );
+
+
+ btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_softBody,m_triBody,0);//m_manifoldPtr);
+ ///this should use the btDispatcher, so the actual registered algorithm is used
+ // btConvexConvexAlgorithm cvxcvxalgo(m_manifoldPtr,ci,m_convexBody,m_triBody);
+
+ //m_resultOut->setShapeIdentifiersB(partId,triangleIndex);
+ // cvxcvxalgo.processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
+ colAlgo->processCollision(m_softBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
+ colAlgo->~btCollisionAlgorithm();
+ ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo);
+
+
+ ob->internalSetTemporaryCollisionShape( tmpShape );
+ triIndex.m_childShape = tm;
+ m_shapeCache.insert(triKey,triIndex);
+
+ }
+
+
+
+}
+
+
+
+void btSoftBodyTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+ m_dispatchInfoPtr = &dispatchInfo;
+ m_collisionMarginTriangle = collisionMarginTriangle+btScalar(BT_SOFTBODY_TRIANGLE_EXTRUSION);
+ m_resultOut = resultOut;
+
+
+ btVector3 aabbWorldSpaceMin,aabbWorldSpaceMax;
+ m_softBody->getAabb(aabbWorldSpaceMin,aabbWorldSpaceMax);
+ btVector3 halfExtents = (aabbWorldSpaceMax-aabbWorldSpaceMin)*btScalar(0.5);
+ btVector3 softBodyCenter = (aabbWorldSpaceMax+aabbWorldSpaceMin)*btScalar(0.5);
+
+ btTransform softTransform;
+ softTransform.setIdentity();
+ softTransform.setOrigin(softBodyCenter);
+
+ btTransform convexInTriangleSpace;
+ convexInTriangleSpace = m_triBody->getWorldTransform().inverse() * softTransform;
+ btTransformAabb(halfExtents,m_collisionMarginTriangle,convexInTriangleSpace,m_aabbMin,m_aabbMax);
+}
+
+void btSoftBodyConcaveCollisionAlgorithm::clearCache()
+{
+ m_btSoftBodyTriangleCallback.clearCache();
+
+}
+
+void btSoftBodyConcaveCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+
+
+ //btCollisionObject* convexBody = m_isSwapped ? body1 : body0;
+ btCollisionObject* triBody = m_isSwapped ? body0 : body1;
+
+ if (triBody->getCollisionShape()->isConcave())
+ {
+
+
+ btCollisionObject* triOb = triBody;
+ btConcaveShape* concaveShape = static_cast<btConcaveShape*>( triOb->getCollisionShape());
+
+ // if (convexBody->getCollisionShape()->isConvex())
+ {
+ btScalar collisionMarginTriangle = concaveShape->getMargin();
+
+ // resultOut->setPersistentManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr);
+ m_btSoftBodyTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,resultOut);
+
+ //Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here.
+ //m_dispatcher->clearManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr);
+
+ // m_btSoftBodyTriangleCallback.m_manifoldPtr->setBodies(convexBody,triBody);
+
+
+ concaveShape->processAllTriangles( &m_btSoftBodyTriangleCallback,m_btSoftBodyTriangleCallback.getAabbMin(),m_btSoftBodyTriangleCallback.getAabbMax());
+
+ // resultOut->refreshContactPoints();
+
+ }
+
+ }
+
+}
+
+
+btScalar btSoftBodyConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+ (void)resultOut;
+ (void)dispatchInfo;
+ btCollisionObject* convexbody = m_isSwapped ? body1 : body0;
+ btCollisionObject* triBody = m_isSwapped ? body0 : body1;
+
+
+ //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)
+
+ //only perform CCD above a certain threshold, this prevents blocking on the long run
+ //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...
+ btScalar squareMot0 = (convexbody->getInterpolationWorldTransform().getOrigin() - convexbody->getWorldTransform().getOrigin()).length2();
+ if (squareMot0 < convexbody->getCcdSquareMotionThreshold())
+ {
+ return btScalar(1.);
+ }
+
+ //const btVector3& from = convexbody->m_worldTransform.getOrigin();
+ //btVector3 to = convexbody->m_interpolationWorldTransform.getOrigin();
+ //todo: only do if the motion exceeds the 'radius'
+
+ btTransform triInv = triBody->getWorldTransform().inverse();
+ btTransform convexFromLocal = triInv * convexbody->getWorldTransform();
+ btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform();
+
+ struct LocalTriangleSphereCastCallback : public btTriangleCallback
+ {
+ btTransform m_ccdSphereFromTrans;
+ btTransform m_ccdSphereToTrans;
+ btTransform m_meshTransform;
+
+ btScalar m_ccdSphereRadius;
+ btScalar m_hitFraction;
+
+
+ LocalTriangleSphereCastCallback(const btTransform& from,const btTransform& to,btScalar ccdSphereRadius,btScalar hitFraction)
+ :m_ccdSphereFromTrans(from),
+ m_ccdSphereToTrans(to),
+ m_ccdSphereRadius(ccdSphereRadius),
+ m_hitFraction(hitFraction)
+ {
+ }
+
+
+ virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
+ {
+ (void)partId;
+ (void)triangleIndex;
+ //do a swept sphere for now
+ btTransform ident;
+ ident.setIdentity();
+ btConvexCast::CastResult castResult;
+ castResult.m_fraction = m_hitFraction;
+ btSphereShape pointShape(m_ccdSphereRadius);
+ btTriangleShape triShape(triangle[0],triangle[1],triangle[2]);
+ btVoronoiSimplexSolver simplexSolver;
+ btSubsimplexConvexCast convexCaster(&pointShape,&triShape,&simplexSolver);
+ //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
+ //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
+ //local space?
+
+ if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans,m_ccdSphereToTrans,
+ ident,ident,castResult))
+ {
+ if (m_hitFraction > castResult.m_fraction)
+ m_hitFraction = castResult.m_fraction;
+ }
+
+ }
+
+ };
+
+
+
+
+
+ if (triBody->getCollisionShape()->isConcave())
+ {
+ btVector3 rayAabbMin = convexFromLocal.getOrigin();
+ rayAabbMin.setMin(convexToLocal.getOrigin());
+ btVector3 rayAabbMax = convexFromLocal.getOrigin();
+ rayAabbMax.setMax(convexToLocal.getOrigin());
+ btScalar ccdRadius0 = convexbody->getCcdSweptSphereRadius();
+ rayAabbMin -= btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
+ rayAabbMax += btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
+
+ btScalar curHitFraction = btScalar(1.); //is this available?
+ LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal,
+ convexbody->getCcdSweptSphereRadius(),curHitFraction);
+
+ raycastCallback.m_hitFraction = convexbody->getHitFraction();
+
+ btCollisionObject* concavebody = triBody;
+
+ btConcaveShape* triangleMesh = (btConcaveShape*) concavebody->getCollisionShape();
+
+ if (triangleMesh)
+ {
+ triangleMesh->processAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax);
+ }
+
+
+
+ if (raycastCallback.m_hitFraction < convexbody->getHitFraction())
+ {
+ convexbody->setHitFraction( raycastCallback.m_hitFraction);
+ return raycastCallback.m_hitFraction;
+ }
+ }
+
+ return btScalar(1.);
+
+}
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btSoftBodyHelpers.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btSoftBodyHelpers.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btSoftBodyHelpers.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btSoftBodyHelpers.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,1006 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+///btSoftBodyHelpers.cpp by Nathanael Presson
+
+#include "BulletSoftBody/btSoftBodyInternals.h"
+#include <stdio.h>
+#include <string.h>
+#include "BulletSoftBody/btSoftBodyHelpers.h"
+#include "LinearMath/btConvexHull.h"
+
+//
+static void drawVertex( btIDebugDraw* idraw,
+ const btVector3& x,btScalar s,const btVector3& c)
+{
+ idraw->drawLine(x-btVector3(s,0,0),x+btVector3(s,0,0),c);
+ idraw->drawLine(x-btVector3(0,s,0),x+btVector3(0,s,0),c);
+ idraw->drawLine(x-btVector3(0,0,s),x+btVector3(0,0,s),c);
+}
+
+//
+static void drawBox( btIDebugDraw* idraw,
+ const btVector3& mins,
+ const btVector3& maxs,
+ const btVector3& color)
+{
+ const btVector3 c[]={ btVector3(mins.x(),mins.y(),mins.z()),
+ btVector3(maxs.x(),mins.y(),mins.z()),
+ btVector3(maxs.x(),maxs.y(),mins.z()),
+ btVector3(mins.x(),maxs.y(),mins.z()),
+ btVector3(mins.x(),mins.y(),maxs.z()),
+ btVector3(maxs.x(),mins.y(),maxs.z()),
+ btVector3(maxs.x(),maxs.y(),maxs.z()),
+ btVector3(mins.x(),maxs.y(),maxs.z())};
+ idraw->drawLine(c[0],c[1],color);idraw->drawLine(c[1],c[2],color);
+ idraw->drawLine(c[2],c[3],color);idraw->drawLine(c[3],c[0],color);
+ idraw->drawLine(c[4],c[5],color);idraw->drawLine(c[5],c[6],color);
+ idraw->drawLine(c[6],c[7],color);idraw->drawLine(c[7],c[4],color);
+ idraw->drawLine(c[0],c[4],color);idraw->drawLine(c[1],c[5],color);
+ idraw->drawLine(c[2],c[6],color);idraw->drawLine(c[3],c[7],color);
+}
+
+//
+static void drawTree( btIDebugDraw* idraw,
+ const btDbvtNode* node,
+ int depth,
+ const btVector3& ncolor,
+ const btVector3& lcolor,
+ int mindepth,
+ int maxdepth)
+{
+ if(node)
+ {
+ if(node->isinternal()&&((depth<maxdepth)||(maxdepth<0)))
+ {
+ drawTree(idraw,node->childs[0],depth+1,ncolor,lcolor,mindepth,maxdepth);
+ drawTree(idraw,node->childs[1],depth+1,ncolor,lcolor,mindepth,maxdepth);
+ }
+ if(depth>=mindepth)
+ {
+ const btScalar scl=(btScalar)(node->isinternal()?1:1);
+ const btVector3 mi=node->volume.Center()-node->volume.Extents()*scl;
+ const btVector3 mx=node->volume.Center()+node->volume.Extents()*scl;
+ drawBox(idraw,mi,mx,node->isleaf()?lcolor:ncolor);
+ }
+ }
+}
+
+//
+template <typename T>
+static inline T sum(const btAlignedObjectArray<T>& items)
+{
+ T v;
+ if(items.size())
+ {
+ v=items[0];
+ for(int i=1,ni=items.size();i<ni;++i)
+ {
+ v+=items[i];
+ }
+ }
+ return(v);
+}
+
+//
+template <typename T,typename Q>
+static inline void add(btAlignedObjectArray<T>& items,const Q& value)
+{
+ for(int i=0,ni=items.size();i<ni;++i)
+ {
+ items[i]+=value;
+ }
+}
+
+//
+template <typename T,typename Q>
+static inline void mul(btAlignedObjectArray<T>& items,const Q& value)
+{
+ for(int i=0,ni=items.size();i<ni;++i)
+ {
+ items[i]*=value;
+ }
+}
+
+//
+template <typename T>
+static inline T average(const btAlignedObjectArray<T>& items)
+{
+ const btScalar n=(btScalar)(items.size()>0?items.size():1);
+ return(sum(items)/n);
+}
+
+//
+static inline btScalar tetravolume(const btVector3& x0,
+ const btVector3& x1,
+ const btVector3& x2,
+ const btVector3& x3)
+{
+ const btVector3 a=x1-x0;
+ const btVector3 b=x2-x0;
+ const btVector3 c=x3-x0;
+ return(btDot(a,btCross(b,c)));
+}
+
+//
+#if 0
+static btVector3 stresscolor(btScalar stress)
+{
+ static const btVector3 spectrum[]= { btVector3(1,0,1),
+ btVector3(0,0,1),
+ btVector3(0,1,1),
+ btVector3(0,1,0),
+ btVector3(1,1,0),
+ btVector3(1,0,0),
+ btVector3(1,0,0)};
+ static const int ncolors=sizeof(spectrum)/sizeof(spectrum[0])-1;
+ static const btScalar one=1;
+ stress=btMax<btScalar>(0,btMin<btScalar>(1,stress))*ncolors;
+ const int sel=(int)stress;
+ const btScalar frc=stress-sel;
+ return(spectrum[sel]+(spectrum[sel+1]-spectrum[sel])*frc);
+}
+#endif
+
+//
+void btSoftBodyHelpers::Draw( btSoftBody* psb,
+ btIDebugDraw* idraw,
+ int drawflags)
+{
+ const btScalar scl=(btScalar)0.1;
+ const btScalar nscl=scl*5;
+ const btVector3 lcolor=btVector3(0,0,0);
+ const btVector3 ncolor=btVector3(1,1,1);
+ const btVector3 ccolor=btVector3(1,0,0);
+ int i,j,nj;
+
+ /* Nodes */
+ if(0!=(drawflags&fDrawFlags::Nodes))
+ {
+ for(i=0;i<psb->m_nodes.size();++i)
+ {
+ const btSoftBody::Node& n=psb->m_nodes[i];
+ if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
+ idraw->drawLine(n.m_x-btVector3(scl,0,0),n.m_x+btVector3(scl,0,0),btVector3(1,0,0));
+ idraw->drawLine(n.m_x-btVector3(0,scl,0),n.m_x+btVector3(0,scl,0),btVector3(0,1,0));
+ idraw->drawLine(n.m_x-btVector3(0,0,scl),n.m_x+btVector3(0,0,scl),btVector3(0,0,1));
+ }
+ }
+ /* Links */
+ if(0!=(drawflags&fDrawFlags::Links))
+ {
+ for(i=0;i<psb->m_links.size();++i)
+ {
+ const btSoftBody::Link& l=psb->m_links[i];
+ if(0==(l.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
+ idraw->drawLine(l.m_n[0]->m_x,l.m_n[1]->m_x,lcolor);
+ }
+ }
+ /* Normals */
+ if(0!=(drawflags&fDrawFlags::Normals))
+ {
+ for(i=0;i<psb->m_nodes.size();++i)
+ {
+ const btSoftBody::Node& n=psb->m_nodes[i];
+ if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
+ const btVector3 d=n.m_n*nscl;
+ idraw->drawLine(n.m_x,n.m_x+d,ncolor);
+ idraw->drawLine(n.m_x,n.m_x-d,ncolor*0.5);
+ }
+ }
+ /* Contacts */
+ if(0!=(drawflags&fDrawFlags::Contacts))
+ {
+ static const btVector3 axis[]={btVector3(1,0,0),
+ btVector3(0,1,0),
+ btVector3(0,0,1)};
+ for(i=0;i<psb->m_rcontacts.size();++i)
+ {
+ const btSoftBody::RContact& c=psb->m_rcontacts[i];
+ const btVector3 o= c.m_node->m_x-c.m_cti.m_normal*
+ (btDot(c.m_node->m_x,c.m_cti.m_normal)+c.m_cti.m_offset);
+ const btVector3 x=btCross(c.m_cti.m_normal,axis[c.m_cti.m_normal.minAxis()]).normalized();
+ const btVector3 y=btCross(x,c.m_cti.m_normal).normalized();
+ idraw->drawLine(o-x*nscl,o+x*nscl,ccolor);
+ idraw->drawLine(o-y*nscl,o+y*nscl,ccolor);
+ idraw->drawLine(o,o+c.m_cti.m_normal*nscl*3,btVector3(1,1,0));
+ }
+ }
+ /* Anchors */
+ if(0!=(drawflags&fDrawFlags::Anchors))
+ {
+ for(i=0;i<psb->m_anchors.size();++i)
+ {
+ const btSoftBody::Anchor& a=psb->m_anchors[i];
+ const btVector3 q=a.m_body->getWorldTransform()*a.m_local;
+ drawVertex(idraw,a.m_node->m_x,0.25,btVector3(1,0,0));
+ drawVertex(idraw,q,0.25,btVector3(0,1,0));
+ idraw->drawLine(a.m_node->m_x,q,btVector3(1,1,1));
+ }
+ for(i=0;i<psb->m_nodes.size();++i)
+ {
+ const btSoftBody::Node& n=psb->m_nodes[i];
+ if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
+ if(n.m_im<=0)
+ {
+ drawVertex(idraw,n.m_x,0.25,btVector3(1,0,0));
+ }
+ }
+ }
+ /* Faces */
+ if(0!=(drawflags&fDrawFlags::Faces))
+ {
+ const btScalar scl=(btScalar)0.8;
+ const btScalar alp=(btScalar)1;
+ const btVector3 col(0,(btScalar)0.7,0);
+ for(i=0;i<psb->m_faces.size();++i)
+ {
+ const btSoftBody::Face& f=psb->m_faces[i];
+ if(0==(f.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
+ const btVector3 x[]={f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x};
+ const btVector3 c=(x[0]+x[1]+x[2])/3;
+ idraw->drawTriangle((x[0]-c)*scl+c,
+ (x[1]-c)*scl+c,
+ (x[2]-c)*scl+c,
+ col,alp);
+ }
+ }
+ /* Clusters */
+ if(0!=(drawflags&fDrawFlags::Clusters))
+ {
+ srand(1806);
+ for(i=0;i<psb->m_clusters.size();++i)
+ {
+ if(psb->m_clusters[i]->m_collide)
+ {
+ btVector3 color( rand()/(btScalar)RAND_MAX,
+ rand()/(btScalar)RAND_MAX,
+ rand()/(btScalar)RAND_MAX);
+ color=color.normalized()*0.75;
+ btAlignedObjectArray<btVector3> vertices;
+ vertices.resize(psb->m_clusters[i]->m_nodes.size());
+ for(j=0,nj=vertices.size();j<nj;++j)
+ {
+ vertices[j]=psb->m_clusters[i]->m_nodes[j]->m_x;
+ }
+ HullDesc hdsc(QF_TRIANGLES,vertices.size(),&vertices[0]);
+ HullResult hres;
+ HullLibrary hlib;
+ hdsc.mMaxVertices=vertices.size();
+ hlib.CreateConvexHull(hdsc,hres);
+ const btVector3 center=average(hres.m_OutputVertices);
+ add(hres.m_OutputVertices,-center);
+ mul(hres.m_OutputVertices,(btScalar)1);
+ add(hres.m_OutputVertices,center);
+ for(j=0;j<(int)hres.mNumFaces;++j)
+ {
+ const int idx[]={hres.m_Indices[j*3+0],hres.m_Indices[j*3+1],hres.m_Indices[j*3+2]};
+ idraw->drawTriangle(hres.m_OutputVertices[idx[0]],
+ hres.m_OutputVertices[idx[1]],
+ hres.m_OutputVertices[idx[2]],
+ color,1);
+ }
+ hlib.ReleaseResult(hres);
+ }
+ /* Velocities */
+#if 0
+ for(int j=0;j<psb->m_clusters[i].m_nodes.size();++j)
+ {
+ const btSoftBody::Cluster& c=psb->m_clusters[i];
+ const btVector3 r=c.m_nodes[j]->m_x-c.m_com;
+ const btVector3 v=c.m_lv+btCross(c.m_av,r);
+ idraw->drawLine(c.m_nodes[j]->m_x,c.m_nodes[j]->m_x+v,btVector3(1,0,0));
+ }
+#endif
+ /* Frame */
+ btSoftBody::Cluster& c=*psb->m_clusters[i];
+ idraw->drawLine(c.m_com,c.m_framexform*btVector3(10,0,0),btVector3(1,0,0));
+ idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,10,0),btVector3(0,1,0));
+ idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,0,10),btVector3(0,0,1));
+ }
+ }
+
+ /* Tetras */
+ if(0!=(drawflags&fDrawFlags::Tetras))
+ {
+ const btScalar scl=(btScalar)0.8;
+ const btScalar alp=(btScalar)1;
+ const btVector3 col((btScalar)0.7,(btScalar)0.7,(btScalar)0.7);
+ for(int i=0;i<psb->m_tetras.size();++i)
+ {
+ const btSoftBody::Tetra& t=psb->m_tetras[i];
+ if(0==(t.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
+ const btVector3 x[]={t.m_n[0]->m_x,t.m_n[1]->m_x,t.m_n[2]->m_x,t.m_n[3]->m_x};
+ const btVector3 c=(x[0]+x[1]+x[2]+x[3])/4;
+ idraw->drawTriangle((x[0]-c)*scl+c,(x[1]-c)*scl+c,(x[2]-c)*scl+c,col,alp);
+ idraw->drawTriangle((x[0]-c)*scl+c,(x[1]-c)*scl+c,(x[3]-c)*scl+c,col,alp);
+ idraw->drawTriangle((x[1]-c)*scl+c,(x[2]-c)*scl+c,(x[3]-c)*scl+c,col,alp);
+ idraw->drawTriangle((x[2]-c)*scl+c,(x[0]-c)*scl+c,(x[3]-c)*scl+c,col,alp);
+ }
+ }
+
+ /* Notes */
+ if(0!=(drawflags&fDrawFlags::Notes))
+ {
+ for(i=0;i<psb->m_notes.size();++i)
+ {
+ const btSoftBody::Note& n=psb->m_notes[i];
+ btVector3 p=n.m_offset;
+ for(int j=0;j<n.m_rank;++j)
+ {
+ p+=n.m_nodes[j]->m_x*n.m_coords[j];
+ }
+ idraw->draw3dText(p,n.m_text);
+ }
+ }
+ /* Node tree */
+ if(0!=(drawflags&fDrawFlags::NodeTree)) DrawNodeTree(psb,idraw);
+ /* Face tree */
+ if(0!=(drawflags&fDrawFlags::FaceTree)) DrawFaceTree(psb,idraw);
+ /* Cluster tree */
+ if(0!=(drawflags&fDrawFlags::ClusterTree)) DrawClusterTree(psb,idraw);
+ /* Joints */
+ if(0!=(drawflags&fDrawFlags::Joints))
+ {
+ for(i=0;i<psb->m_joints.size();++i)
+ {
+ const btSoftBody::Joint* pj=psb->m_joints[i];
+ switch(pj->Type())
+ {
+ case btSoftBody::Joint::eType::Linear:
+ {
+ const btSoftBody::LJoint* pjl=(const btSoftBody::LJoint*)pj;
+ const btVector3 a0=pj->m_bodies[0].xform()*pjl->m_refs[0];
+ const btVector3 a1=pj->m_bodies[1].xform()*pjl->m_refs[1];
+ idraw->drawLine(pj->m_bodies[0].xform().getOrigin(),a0,btVector3(1,1,0));
+ idraw->drawLine(pj->m_bodies[1].xform().getOrigin(),a1,btVector3(0,1,1));
+ drawVertex(idraw,a0,0.25,btVector3(1,1,0));
+ drawVertex(idraw,a1,0.25,btVector3(0,1,1));
+ }
+ break;
+ case btSoftBody::Joint::eType::Angular:
+ {
+ //const btSoftBody::AJoint* pja=(const btSoftBody::AJoint*)pj;
+ const btVector3 o0=pj->m_bodies[0].xform().getOrigin();
+ const btVector3 o1=pj->m_bodies[1].xform().getOrigin();
+ const btVector3 a0=pj->m_bodies[0].xform().getBasis()*pj->m_refs[0];
+ const btVector3 a1=pj->m_bodies[1].xform().getBasis()*pj->m_refs[1];
+ idraw->drawLine(o0,o0+a0*10,btVector3(1,1,0));
+ idraw->drawLine(o0,o0+a1*10,btVector3(1,1,0));
+ idraw->drawLine(o1,o1+a0*10,btVector3(0,1,1));
+ idraw->drawLine(o1,o1+a1*10,btVector3(0,1,1));
+ }
+ }
+ }
+ }
+}
+
+//
+void btSoftBodyHelpers::DrawInfos( btSoftBody* psb,
+ btIDebugDraw* idraw,
+ bool masses,
+ bool areas,
+ bool /*stress*/)
+{
+ for(int i=0;i<psb->m_nodes.size();++i)
+ {
+ const btSoftBody::Node& n=psb->m_nodes[i];
+ char text[2048]={0};
+ char buff[1024];
+ if(masses)
+ {
+ sprintf(buff," M(%.2f)",1/n.m_im);
+ strcat(text,buff);
+ }
+ if(areas)
+ {
+ sprintf(buff," A(%.2f)",n.m_area);
+ strcat(text,buff);
+ }
+ if(text[0]) idraw->draw3dText(n.m_x,text);
+ }
+}
+
+//
+void btSoftBodyHelpers::DrawNodeTree( btSoftBody* psb,
+ btIDebugDraw* idraw,
+ int mindepth,
+ int maxdepth)
+{
+ drawTree(idraw,psb->m_ndbvt.m_root,0,btVector3(1,0,1),btVector3(1,1,1),mindepth,maxdepth);
+}
+
+//
+void btSoftBodyHelpers::DrawFaceTree( btSoftBody* psb,
+ btIDebugDraw* idraw,
+ int mindepth,
+ int maxdepth)
+{
+ drawTree(idraw,psb->m_fdbvt.m_root,0,btVector3(0,1,0),btVector3(1,0,0),mindepth,maxdepth);
+}
+
+//
+void btSoftBodyHelpers::DrawClusterTree( btSoftBody* psb,
+ btIDebugDraw* idraw,
+ int mindepth,
+ int maxdepth)
+{
+ drawTree(idraw,psb->m_cdbvt.m_root,0,btVector3(0,1,1),btVector3(1,0,0),mindepth,maxdepth);
+}
+
+//
+void btSoftBodyHelpers::DrawFrame( btSoftBody* psb,
+ btIDebugDraw* idraw)
+{
+ if(psb->m_pose.m_bframe)
+ {
+ static const btScalar ascl=10;
+ static const btScalar nscl=(btScalar)0.1;
+ const btVector3 com=psb->m_pose.m_com;
+ const btMatrix3x3 trs=psb->m_pose.m_rot*psb->m_pose.m_scl;
+ const btVector3 Xaxis=(trs*btVector3(1,0,0)).normalized();
+ const btVector3 Yaxis=(trs*btVector3(0,1,0)).normalized();
+ const btVector3 Zaxis=(trs*btVector3(0,0,1)).normalized();
+ idraw->drawLine(com,com+Xaxis*ascl,btVector3(1,0,0));
+ idraw->drawLine(com,com+Yaxis*ascl,btVector3(0,1,0));
+ idraw->drawLine(com,com+Zaxis*ascl,btVector3(0,0,1));
+ for(int i=0;i<psb->m_pose.m_pos.size();++i)
+ {
+ const btVector3 x=com+trs*psb->m_pose.m_pos[i];
+ drawVertex(idraw,x,nscl,btVector3(1,0,1));
+ }
+ }
+}
+
+//
+btSoftBody* btSoftBodyHelpers::CreateRope( btSoftBodyWorldInfo& worldInfo, const btVector3& from,
+ const btVector3& to,
+ int res,
+ int fixeds)
+{
+ /* Create nodes */
+ const int r=res+2;
+ btVector3* x=new btVector3[r];
+ btScalar* m=new btScalar[r];
+ int i;
+
+ for(i=0;i<r;++i)
+ {
+ const btScalar t=i/(btScalar)(r-1);
+ x[i]=lerp(from,to,t);
+ m[i]=1;
+ }
+ btSoftBody* psb= new btSoftBody(&worldInfo,r,x,m);
+ if(fixeds&1) psb->setMass(0,0);
+ if(fixeds&2) psb->setMass(r-1,0);
+ delete[] x;
+ delete[] m;
+ /* Create links */
+ for(i=1;i<r;++i)
+ {
+ psb->appendLink(i-1,i);
+ }
+ /* Finished */
+ return(psb);
+}
+
+//
+btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBodyWorldInfo& worldInfo,const btVector3& corner00,
+ const btVector3& corner10,
+ const btVector3& corner01,
+ const btVector3& corner11,
+ int resx,
+ int resy,
+ int fixeds,
+ bool gendiags)
+{
+#define IDX(_x_,_y_) ((_y_)*rx+(_x_))
+ /* Create nodes */
+ if((resx<2)||(resy<2)) return(0);
+ const int rx=resx;
+ const int ry=resy;
+ const int tot=rx*ry;
+ btVector3* x=new btVector3[tot];
+ btScalar* m=new btScalar[tot];
+ int iy;
+
+ for(iy=0;iy<ry;++iy)
+ {
+ const btScalar ty=iy/(btScalar)(ry-1);
+ const btVector3 py0=lerp(corner00,corner01,ty);
+ const btVector3 py1=lerp(corner10,corner11,ty);
+ for(int ix=0;ix<rx;++ix)
+ {
+ const btScalar tx=ix/(btScalar)(rx-1);
+ x[IDX(ix,iy)]=lerp(py0,py1,tx);
+ m[IDX(ix,iy)]=1;
+ }
+ }
+ btSoftBody* psb=new btSoftBody(&worldInfo,tot,x,m);
+ if(fixeds&1) psb->setMass(IDX(0,0),0);
+ if(fixeds&2) psb->setMass(IDX(rx-1,0),0);
+ if(fixeds&4) psb->setMass(IDX(0,ry-1),0);
+ if(fixeds&8) psb->setMass(IDX(rx-1,ry-1),0);
+ delete[] x;
+ delete[] m;
+ /* Create links and faces */
+ for(iy=0;iy<ry;++iy)
+ {
+ for(int ix=0;ix<rx;++ix)
+ {
+ const int idx=IDX(ix,iy);
+ const bool mdx=(ix+1)<rx;
+ const bool mdy=(iy+1)<ry;
+ if(mdx) psb->appendLink(idx,IDX(ix+1,iy));
+ if(mdy) psb->appendLink(idx,IDX(ix,iy+1));
+ if(mdx&&mdy)
+ {
+ if((ix+iy)&1)
+ {
+ psb->appendFace(IDX(ix,iy),IDX(ix+1,iy),IDX(ix+1,iy+1));
+ psb->appendFace(IDX(ix,iy),IDX(ix+1,iy+1),IDX(ix,iy+1));
+ if(gendiags)
+ {
+ psb->appendLink(IDX(ix,iy),IDX(ix+1,iy+1));
+ }
+ }
+ else
+ {
+ psb->appendFace(IDX(ix,iy+1),IDX(ix,iy),IDX(ix+1,iy));
+ psb->appendFace(IDX(ix,iy+1),IDX(ix+1,iy),IDX(ix+1,iy+1));
+ if(gendiags)
+ {
+ psb->appendLink(IDX(ix+1,iy),IDX(ix,iy+1));
+ }
+ }
+ }
+ }
+ }
+ /* Finished */
+#undef IDX
+ return(psb);
+}
+
+//
+btSoftBody* btSoftBodyHelpers::CreatePatchUV(btSoftBodyWorldInfo& worldInfo,
+ const btVector3& corner00,
+ const btVector3& corner10,
+ const btVector3& corner01,
+ const btVector3& corner11,
+ int resx,
+ int resy,
+ int fixeds,
+ bool gendiags,
+ float* tex_coords)
+{
+
+ /*
+ *
+ * corners:
+ *
+ * [0][0] corner00 ------- corner01 [resx][0]
+ * | |
+ * | |
+ * [0][resy] corner10 -------- corner11 [resx][resy]
+ *
+ *
+ *
+ *
+ *
+ *
+ * "fixedgs" map:
+ *
+ * corner00 --> +1
+ * corner01 --> +2
+ * corner10 --> +4
+ * corner11 --> +8
+ * upper middle --> +16
+ * left middle --> +32
+ * right middle --> +64
+ * lower middle --> +128
+ * center --> +256
+ *
+ *
+ * tex_coords size (resx-1)*(resy-1)*12
+ *
+ *
+ *
+ * SINGLE QUAD INTERNALS
+ *
+ * 1) btSoftBody's nodes and links,
+ * diagonal link is optional ("gendiags")
+ *
+ *
+ * node00 ------ node01
+ * | .
+ * | .
+ * | .
+ * | .
+ * | .
+ * node10 node11
+ *
+ *
+ *
+ * 2) Faces:
+ * two triangles,
+ * UV Coordinates (hier example for single quad)
+ *
+ * (0,1) (0,1) (1,1)
+ * 1 |\ 3 \-----| 2
+ * | \ \ |
+ * | \ \ |
+ * | \ \ |
+ * | \ \ |
+ * 2 |-----\ 3 \| 1
+ * (0,0) (1,0) (1,0)
+ *
+ *
+ *
+ *
+ *
+ *
+ */
+
+#define IDX(_x_,_y_) ((_y_)*rx+(_x_))
+ /* Create nodes */
+ if((resx<2)||(resy<2)) return(0);
+ const int rx=resx;
+ const int ry=resy;
+ const int tot=rx*ry;
+ btVector3* x=new btVector3[tot];
+ btScalar* m=new btScalar[tot];
+
+ int iy;
+
+ for(iy=0;iy<ry;++iy)
+ {
+ const btScalar ty=iy/(btScalar)(ry-1);
+ const btVector3 py0=lerp(corner00,corner01,ty);
+ const btVector3 py1=lerp(corner10,corner11,ty);
+ for(int ix=0;ix<rx;++ix)
+ {
+ const btScalar tx=ix/(btScalar)(rx-1);
+ x[IDX(ix,iy)]=lerp(py0,py1,tx);
+ m[IDX(ix,iy)]=1;
+ }
+ }
+ btSoftBody* psb=new btSoftBody(&worldInfo,tot,x,m);
+ if(fixeds&1) psb->setMass(IDX(0,0),0);
+ if(fixeds&2) psb->setMass(IDX(rx-1,0),0);
+ if(fixeds&4) psb->setMass(IDX(0,ry-1),0);
+ if(fixeds&8) psb->setMass(IDX(rx-1,ry-1),0);
+ if(fixeds&16) psb->setMass(IDX((rx-1)/2,0),0);
+ if(fixeds&32) psb->setMass(IDX(0,(ry-1)/2),0);
+ if(fixeds&64) psb->setMass(IDX(rx-1,(ry-1)/2),0);
+ if(fixeds&128) psb->setMass(IDX((rx-1)/2,ry-1),0);
+ if(fixeds&256) psb->setMass(IDX((rx-1)/2,(ry-1)/2),0);
+ delete[] x;
+ delete[] m;
+
+
+ int z = 0;
+ /* Create links and faces */
+ for(iy=0;iy<ry;++iy)
+ {
+ for(int ix=0;ix<rx;++ix)
+ {
+ const bool mdx=(ix+1)<rx;
+ const bool mdy=(iy+1)<ry;
+
+ int node00=IDX(ix,iy);
+ int node01=IDX(ix+1,iy);
+ int node10=IDX(ix,iy+1);
+ int node11=IDX(ix+1,iy+1);
+
+ if(mdx) psb->appendLink(node00,node01);
+ if(mdy) psb->appendLink(node00,node10);
+ if(mdx&&mdy)
+ {
+ psb->appendFace(node00,node10,node11);
+ if (tex_coords) {
+ tex_coords[z+0]=CalculateUV(resx,resy,ix,iy,0);
+ tex_coords[z+1]=CalculateUV(resx,resy,ix,iy,1);
+ tex_coords[z+2]=CalculateUV(resx,resy,ix,iy,0);
+ tex_coords[z+3]=CalculateUV(resx,resy,ix,iy,2);
+ tex_coords[z+4]=CalculateUV(resx,resy,ix,iy,3);
+ tex_coords[z+5]=CalculateUV(resx,resy,ix,iy,2);
+ }
+ psb->appendFace(node11,node01,node00);
+ if (tex_coords) {
+ tex_coords[z+6 ]=CalculateUV(resx,resy,ix,iy,3);
+ tex_coords[z+7 ]=CalculateUV(resx,resy,ix,iy,2);
+ tex_coords[z+8 ]=CalculateUV(resx,resy,ix,iy,3);
+ tex_coords[z+9 ]=CalculateUV(resx,resy,ix,iy,1);
+ tex_coords[z+10]=CalculateUV(resx,resy,ix,iy,0);
+ tex_coords[z+11]=CalculateUV(resx,resy,ix,iy,1);
+ }
+ if (gendiags) psb->appendLink(node00,node11);
+ z += 12;
+ }
+ }
+ }
+ /* Finished */
+#undef IDX
+ return(psb);
+}
+
+float btSoftBodyHelpers::CalculateUV(int resx,int resy,int ix,int iy,int id)
+{
+
+ /*
+ *
+ *
+ * node00 --- node01
+ * | |
+ * node10 --- node11
+ *
+ *
+ * ID map:
+ *
+ * node00 s --> 0
+ * node00 t --> 1
+ *
+ * node01 s --> 3
+ * node01 t --> 1
+ *
+ * node10 s --> 0
+ * node10 t --> 2
+ *
+ * node11 s --> 3
+ * node11 t --> 2
+ *
+ *
+ */
+
+ float tc=0.0f;
+ if (id == 0) {
+ tc = (1.0f/((resx-1))*ix);
+ }
+ else if (id==1) {
+ tc = (1.0f/((resy-1))*(resy-1-iy));
+ }
+ else if (id==2) {
+ tc = (1.0f/((resy-1))*(resy-1-iy-1));
+ }
+ else if (id==3) {
+ tc = (1.0f/((resx-1))*(ix+1));
+ }
+ return tc;
+}
+//
+btSoftBody* btSoftBodyHelpers::CreateEllipsoid(btSoftBodyWorldInfo& worldInfo,const btVector3& center,
+ const btVector3& radius,
+ int res)
+{
+ struct Hammersley
+ {
+ static void Generate(btVector3* x,int n)
+ {
+ for(int i=0;i<n;i++)
+ {
+ btScalar p=0.5,t=0;
+ for(int j=i;j;p*=0.5,j>>=1) if(j&1) t+=p;
+ btScalar w=2*t-1;
+ btScalar a=(SIMD_PI+2*i*SIMD_PI)/n;
+ btScalar s=btSqrt(1-w*w);
+ *x++=btVector3(s*btCos(a),s*btSin(a),w);
+ }
+ }
+ };
+ btAlignedObjectArray<btVector3> vtx;
+ vtx.resize(3+res);
+ Hammersley::Generate(&vtx[0],vtx.size());
+ for(int i=0;i<vtx.size();++i)
+ {
+ vtx[i]=vtx[i]*radius+center;
+ }
+ return(CreateFromConvexHull(worldInfo,&vtx[0],vtx.size()));
+}
+
+
+
+//
+btSoftBody* btSoftBodyHelpers::CreateFromTriMesh(btSoftBodyWorldInfo& worldInfo,const btScalar* vertices,
+ const int* triangles,
+ int ntriangles)
+{
+ int maxidx=0;
+ int i,j,ni;
+
+ for(i=0,ni=ntriangles*3;i<ni;++i)
+ {
+ maxidx=btMax(triangles[i],maxidx);
+ }
+ ++maxidx;
+ btAlignedObjectArray<bool> chks;
+ btAlignedObjectArray<btVector3> vtx;
+ chks.resize(maxidx*maxidx,false);
+ vtx.resize(maxidx);
+ for(i=0,j=0,ni=maxidx*3;i<ni;++j,i+=3)
+ {
+ vtx[j]=btVector3(vertices[i],vertices[i+1],vertices[i+2]);
+ }
+ btSoftBody* psb=new btSoftBody(&worldInfo,vtx.size(),&vtx[0],0);
+ for( i=0,ni=ntriangles*3;i<ni;i+=3)
+ {
+ const int idx[]={triangles[i],triangles[i+1],triangles[i+2]};
+#define IDX(_x_,_y_) ((_y_)*maxidx+(_x_))
+ for(int j=2,k=0;k<3;j=k++)
+ {
+ if(!chks[IDX(idx[j],idx[k])])
+ {
+ chks[IDX(idx[j],idx[k])]=true;
+ chks[IDX(idx[k],idx[j])]=true;
+ psb->appendLink(idx[j],idx[k]);
+ }
+ }
+#undef IDX
+ psb->appendFace(idx[0],idx[1],idx[2]);
+ }
+ psb->randomizeConstraints();
+ return(psb);
+}
+
+//
+btSoftBody* btSoftBodyHelpers::CreateFromConvexHull(btSoftBodyWorldInfo& worldInfo, const btVector3* vertices,
+ int nvertices)
+{
+ HullDesc hdsc(QF_TRIANGLES,nvertices,vertices);
+ HullResult hres;
+ HullLibrary hlib;/*??*/
+ hdsc.mMaxVertices=nvertices;
+ hlib.CreateConvexHull(hdsc,hres);
+ btSoftBody* psb=new btSoftBody(&worldInfo,(int)hres.mNumOutputVertices,
+ &hres.m_OutputVertices[0],0);
+ for(int i=0;i<(int)hres.mNumFaces;++i)
+ {
+ const int idx[]={ hres.m_Indices[i*3+0],
+ hres.m_Indices[i*3+1],
+ hres.m_Indices[i*3+2]};
+ if(idx[0]<idx[1]) psb->appendLink( idx[0],idx[1]);
+ if(idx[1]<idx[2]) psb->appendLink( idx[1],idx[2]);
+ if(idx[2]<idx[0]) psb->appendLink( idx[2],idx[0]);
+ psb->appendFace(idx[0],idx[1],idx[2]);
+ }
+ hlib.ReleaseResult(hres);
+ psb->randomizeConstraints();
+ return(psb);
+}
+
+
+
+
+static int nextLine(const char* buffer)
+{
+ int numBytesRead=0;
+
+ while (*buffer != '\n')
+ {
+ buffer++;
+ numBytesRead++;
+ }
+
+
+ if (buffer[0]==0x0a)
+ {
+ buffer++;
+ numBytesRead++;
+ }
+ return numBytesRead;
+}
+
+/* Create from TetGen .ele, .face, .node data */
+btSoftBody* btSoftBodyHelpers::CreateFromTetGenData(btSoftBodyWorldInfo& worldInfo,
+ const char* ele,
+ const char* face,
+ const char* node,
+ bool bfacelinks,
+ bool btetralinks,
+ bool bfacesfromtetras)
+{
+btAlignedObjectArray<btVector3> pos;
+int nnode=0;
+int ndims=0;
+int nattrb=0;
+int hasbounds=0;
+int result = sscanf(node,"%d %d %d %d",&nnode,&ndims,&nattrb,&hasbounds);
+result = sscanf(node,"%d %d %d %d",&nnode,&ndims,&nattrb,&hasbounds);
+node += nextLine(node);
+
+pos.resize(nnode);
+for(int i=0;i<pos.size();++i)
+ {
+ int index=0;
+ //int bound=0;
+ float x,y,z;
+ sscanf(node,"%d %f %f %f",&index,&x,&y,&z);
+
+// sn>>index;
+// sn>>x;sn>>y;sn>>z;
+ node += nextLine(node);
+
+ //for(int j=0;j<nattrb;++j)
+ // sn>>a;
+
+ //if(hasbounds)
+ // sn>>bound;
+
+ pos[index].setX(btScalar(x));
+ pos[index].setY(btScalar(y));
+ pos[index].setZ(btScalar(z));
+ }
+btSoftBody* psb=new btSoftBody(&worldInfo,nnode,&pos[0],0);
+#if 0
+if(face&&face[0])
+ {
+ int nface=0;
+ sf>>nface;sf>>hasbounds;
+ for(int i=0;i<nface;++i)
+ {
+ int index=0;
+ int bound=0;
+ int ni[3];
+ sf>>index;
+ sf>>ni[0];sf>>ni[1];sf>>ni[2];
+ sf>>bound;
+ psb->appendFace(ni[0],ni[1],ni[2]);
+ if(btetralinks)
+ {
+ psb->appendLink(ni[0],ni[1],0,true);
+ psb->appendLink(ni[1],ni[2],0,true);
+ psb->appendLink(ni[2],ni[0],0,true);
+ }
+ }
+ }
+#endif
+
+if(ele&&ele[0])
+ {
+ int ntetra=0;
+ int ncorner=0;
+ int neattrb=0;
+ sscanf(ele,"%d %d %d",&ntetra,&ncorner,&neattrb);
+ ele += nextLine(ele);
+
+ //se>>ntetra;se>>ncorner;se>>neattrb;
+ for(int i=0;i<ntetra;++i)
+ {
+ int index=0;
+ int ni[4];
+
+ //se>>index;
+ //se>>ni[0];se>>ni[1];se>>ni[2];se>>ni[3];
+ sscanf(ele,"%d %d %d %d %d",&index,&ni[0],&ni[1],&ni[2],&ni[3]);
+ ele+=nextLine(ele);
+ //for(int j=0;j<neattrb;++j)
+ // se>>a;
+ psb->appendTetra(ni[0],ni[1],ni[2],ni[3]);
+ if(btetralinks)
+ {
+ psb->appendLink(ni[0],ni[1],0,true);
+ psb->appendLink(ni[1],ni[2],0,true);
+ psb->appendLink(ni[2],ni[0],0,true);
+ psb->appendLink(ni[0],ni[3],0,true);
+ psb->appendLink(ni[1],ni[3],0,true);
+ psb->appendLink(ni[2],ni[3],0,true);
+ }
+ }
+ }
+printf("Nodes: %u\r\n",psb->m_nodes.size());
+printf("Links: %u\r\n",psb->m_links.size());
+printf("Faces: %u\r\n",psb->m_faces.size());
+printf("Tetras: %u\r\n",psb->m_tetras.size());
+return(psb);
+}
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btSoftBodyRigidBodyCollisionConfiguration.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btSoftBodyRigidBodyCollisionConfiguration.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btSoftBodyRigidBodyCollisionConfiguration.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btSoftBodyRigidBodyCollisionConfiguration.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,134 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
+#include "BulletSoftBody/btSoftRigidCollisionAlgorithm.h"
+#include "BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h"
+#include "BulletSoftBody/btSoftSoftCollisionAlgorithm.h"
+
+#include "LinearMath/btPoolAllocator.h"
+
+#define ENABLE_SOFTBODY_CONCAVE_COLLISIONS 1
+
+btSoftBodyRigidBodyCollisionConfiguration::btSoftBodyRigidBodyCollisionConfiguration(const btDefaultCollisionConstructionInfo& constructionInfo)
+:btDefaultCollisionConfiguration(constructionInfo)
+{
+ void* mem;
+
+ mem = btAlignedAlloc(sizeof(btSoftSoftCollisionAlgorithm::CreateFunc),16);
+ m_softSoftCreateFunc = new(mem) btSoftSoftCollisionAlgorithm::CreateFunc;
+
+ mem = btAlignedAlloc(sizeof(btSoftRigidCollisionAlgorithm::CreateFunc),16);
+ m_softRigidConvexCreateFunc = new(mem) btSoftRigidCollisionAlgorithm::CreateFunc;
+
+ mem = btAlignedAlloc(sizeof(btSoftRigidCollisionAlgorithm::CreateFunc),16);
+ m_swappedSoftRigidConvexCreateFunc = new(mem) btSoftRigidCollisionAlgorithm::CreateFunc;
+ m_swappedSoftRigidConvexCreateFunc->m_swapped=true;
+
+#ifdef ENABLE_SOFTBODY_CONCAVE_COLLISIONS
+ mem = btAlignedAlloc(sizeof(btSoftBodyConcaveCollisionAlgorithm::CreateFunc),16);
+ m_softRigidConcaveCreateFunc = new(mem) btSoftBodyConcaveCollisionAlgorithm::CreateFunc;
+
+ mem = btAlignedAlloc(sizeof(btSoftBodyConcaveCollisionAlgorithm::CreateFunc),16);
+ m_swappedSoftRigidConcaveCreateFunc = new(mem) btSoftBodyConcaveCollisionAlgorithm::SwappedCreateFunc;
+ m_swappedSoftRigidConcaveCreateFunc->m_swapped=true;
+#endif
+
+ //replace pool by a new one, with potential larger size
+
+ if (m_ownsCollisionAlgorithmPool && m_collisionAlgorithmPool)
+ {
+ int curElemSize = m_collisionAlgorithmPool->getElementSize();
+ ///calculate maximum element size, big enough to fit any collision algorithm in the memory pool
+
+
+ int maxSize0 = sizeof(btSoftSoftCollisionAlgorithm);
+ int maxSize1 = sizeof(btSoftRigidCollisionAlgorithm);
+ int maxSize2 = sizeof(btSoftBodyConcaveCollisionAlgorithm);
+
+ int collisionAlgorithmMaxElementSize = btMax(maxSize0,maxSize1);
+ collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize2);
+
+ if (collisionAlgorithmMaxElementSize > curElemSize)
+ {
+ m_collisionAlgorithmPool->~btPoolAllocator();
+ btAlignedFree(m_collisionAlgorithmPool);
+ void* mem = btAlignedAlloc(sizeof(btPoolAllocator),16);
+ m_collisionAlgorithmPool = new(mem) btPoolAllocator(collisionAlgorithmMaxElementSize,constructionInfo.m_defaultMaxCollisionAlgorithmPoolSize);
+ }
+ }
+
+}
+
+btSoftBodyRigidBodyCollisionConfiguration::~btSoftBodyRigidBodyCollisionConfiguration()
+{
+ m_softSoftCreateFunc->~btCollisionAlgorithmCreateFunc();
+ btAlignedFree( m_softSoftCreateFunc);
+
+ m_softRigidConvexCreateFunc->~btCollisionAlgorithmCreateFunc();
+ btAlignedFree( m_softRigidConvexCreateFunc);
+
+ m_swappedSoftRigidConvexCreateFunc->~btCollisionAlgorithmCreateFunc();
+ btAlignedFree( m_swappedSoftRigidConvexCreateFunc);
+
+#ifdef ENABLE_SOFTBODY_CONCAVE_COLLISIONS
+ m_softRigidConcaveCreateFunc->~btCollisionAlgorithmCreateFunc();
+ btAlignedFree( m_softRigidConcaveCreateFunc);
+
+ m_swappedSoftRigidConcaveCreateFunc->~btCollisionAlgorithmCreateFunc();
+ btAlignedFree( m_swappedSoftRigidConcaveCreateFunc);
+#endif
+}
+
+///creation of soft-soft and soft-rigid, and otherwise fallback to base class implementation
+btCollisionAlgorithmCreateFunc* btSoftBodyRigidBodyCollisionConfiguration::getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1)
+{
+
+ ///try to handle the softbody interactions first
+
+ if ((proxyType0 == SOFTBODY_SHAPE_PROXYTYPE ) && (proxyType1==SOFTBODY_SHAPE_PROXYTYPE))
+ {
+ return m_softSoftCreateFunc;
+ }
+
+ ///softbody versus convex
+ if (proxyType0 == SOFTBODY_SHAPE_PROXYTYPE && btBroadphaseProxy::isConvex(proxyType1))
+ {
+ return m_softRigidConvexCreateFunc;
+ }
+
+ ///convex versus soft body
+ if (btBroadphaseProxy::isConvex(proxyType0) && proxyType1 == SOFTBODY_SHAPE_PROXYTYPE )
+ {
+ return m_swappedSoftRigidConvexCreateFunc;
+ }
+
+#ifdef ENABLE_SOFTBODY_CONCAVE_COLLISIONS
+ ///softbody versus convex
+ if (proxyType0 == SOFTBODY_SHAPE_PROXYTYPE && btBroadphaseProxy::isConcave(proxyType1))
+ {
+ return m_softRigidConcaveCreateFunc;
+ }
+
+ ///convex versus soft body
+ if (btBroadphaseProxy::isConcave(proxyType0) && proxyType1 == SOFTBODY_SHAPE_PROXYTYPE )
+ {
+ return m_swappedSoftRigidConcaveCreateFunc;
+ }
+#endif
+
+ ///fallback to the regular rigid collision shape
+ return btDefaultCollisionConfiguration::getCollisionAlgorithmCreateFunc(proxyType0,proxyType1);
+}
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btSoftRigidCollisionAlgorithm.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btSoftRigidCollisionAlgorithm.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btSoftRigidCollisionAlgorithm.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btSoftRigidCollisionAlgorithm.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,82 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "BulletSoftBody/btSoftRigidCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+#include "BulletCollision/CollisionShapes/btBoxShape.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletSoftBody/btSoftBody.h"
+///TODO: include all the shapes that the softbody can collide with
+///alternatively, implement special case collision algorithms (just like for rigid collision shapes)
+
+//#include <stdio.h>
+
+btSoftRigidCollisionAlgorithm::btSoftRigidCollisionAlgorithm(btPersistentManifold* /*mf*/,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* /*col0*/,btCollisionObject* /*col1*/, bool isSwapped)
+: btCollisionAlgorithm(ci),
+//m_ownManifold(false),
+//m_manifoldPtr(mf),
+m_isSwapped(isSwapped)
+{
+}
+
+
+btSoftRigidCollisionAlgorithm::~btSoftRigidCollisionAlgorithm()
+{
+
+ //m_softBody->m_overlappingRigidBodies.remove(m_rigidCollisionObject);
+
+ /*if (m_ownManifold)
+ {
+ if (m_manifoldPtr)
+ m_dispatcher->releaseManifold(m_manifoldPtr);
+ }
+ */
+
+}
+
+
+#include <stdio.h>
+
+void btSoftRigidCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+ (void)dispatchInfo;
+ (void)resultOut;
+ //printf("btSoftRigidCollisionAlgorithm\n");
+
+ btSoftBody* softBody = m_isSwapped? (btSoftBody*)body1 : (btSoftBody*)body0;
+ btCollisionObject* rigidCollisionObject = m_isSwapped? body0 : body1;
+
+ if (softBody->m_collisionDisabledObjects.findLinearSearch(rigidCollisionObject)==softBody->m_collisionDisabledObjects.size())
+ {
+ softBody->defaultCollisionHandler(rigidCollisionObject);
+ }
+
+
+}
+
+btScalar btSoftRigidCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+ (void)resultOut;
+ (void)dispatchInfo;
+ (void)col0;
+ (void)col1;
+
+ //not yet
+ return btScalar(1.);
+}
+
+
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btSoftRigidDynamicsWorld.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btSoftRigidDynamicsWorld.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btSoftRigidDynamicsWorld.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btSoftRigidDynamicsWorld.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,151 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "BulletSoftBody/btSoftRigidDynamicsWorld.h"
+#include "LinearMath/btQuickprof.h"
+
+//softbody & helpers
+#include "BulletSoftBody/btSoftBody.h"
+#include "BulletSoftBody/btSoftBodyHelpers.h"
+
+
+
+
+
+btSoftRigidDynamicsWorld::btSoftRigidDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration)
+:btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration)
+{
+ m_drawFlags = fDrawFlags::Std;
+ m_drawNodeTree = true;
+ m_drawFaceTree = false;
+ m_drawClusterTree = false;
+ m_sbi.m_broadphase = pairCache;
+ m_sbi.m_dispatcher = dispatcher;
+ m_sbi.m_sparsesdf.Initialize();
+ m_sbi.m_sparsesdf.Reset();
+
+}
+
+btSoftRigidDynamicsWorld::~btSoftRigidDynamicsWorld()
+{
+
+}
+
+void btSoftRigidDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
+{
+ btDiscreteDynamicsWorld::predictUnconstraintMotion( timeStep);
+
+ for ( int i=0;i<m_softBodies.size();++i)
+ {
+ btSoftBody* psb= m_softBodies[i];
+
+ psb->predictMotion(timeStep);
+ }
+}
+
+void btSoftRigidDynamicsWorld::internalSingleStepSimulation( btScalar timeStep)
+{
+ btDiscreteDynamicsWorld::internalSingleStepSimulation( timeStep );
+
+ ///solve soft bodies constraints
+ solveSoftBodiesConstraints();
+
+ //self collisions
+ for ( int i=0;i<m_softBodies.size();i++)
+ {
+ btSoftBody* psb=(btSoftBody*)m_softBodies[i];
+ psb->defaultCollisionHandler(psb);
+ }
+
+ ///update soft bodies
+ updateSoftBodies();
+
+}
+
+void btSoftRigidDynamicsWorld::updateSoftBodies()
+{
+ BT_PROFILE("updateSoftBodies");
+
+ for ( int i=0;i<m_softBodies.size();i++)
+ {
+ btSoftBody* psb=(btSoftBody*)m_softBodies[i];
+ psb->integrateMotion();
+ }
+}
+
+void btSoftRigidDynamicsWorld::solveSoftBodiesConstraints()
+{
+ BT_PROFILE("solveSoftConstraints");
+
+ if(m_softBodies.size())
+ {
+ btSoftBody::solveClusters(m_softBodies);
+ }
+
+ for(int i=0;i<m_softBodies.size();++i)
+ {
+ btSoftBody* psb=(btSoftBody*)m_softBodies[i];
+ psb->solveConstraints();
+ }
+}
+
+void btSoftRigidDynamicsWorld::addSoftBody(btSoftBody* body,short int collisionFilterGroup,short int collisionFilterMask)
+{
+ m_softBodies.push_back(body);
+
+ btCollisionWorld::addCollisionObject(body,
+ collisionFilterGroup,
+ collisionFilterMask);
+
+}
+
+void btSoftRigidDynamicsWorld::removeSoftBody(btSoftBody* body)
+{
+ m_softBodies.remove(body);
+
+ btCollisionWorld::removeCollisionObject(body);
+}
+
+void btSoftRigidDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject)
+{
+ btSoftBody* body = btSoftBody::upcast(collisionObject);
+ if (body)
+ removeSoftBody(body);
+ else
+ btDiscreteDynamicsWorld::removeCollisionObject(collisionObject);
+}
+
+void btSoftRigidDynamicsWorld::debugDrawWorld()
+{
+ btDiscreteDynamicsWorld::debugDrawWorld();
+
+ if (getDebugDrawer())
+ {
+ int i;
+ for ( i=0;i<this->m_softBodies.size();i++)
+ {
+ btSoftBody* psb=(btSoftBody*)this->m_softBodies[i];
+ btSoftBodyHelpers::DrawFrame(psb,m_debugDrawer);
+ btSoftBodyHelpers::Draw(psb,m_debugDrawer,m_drawFlags);
+ if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
+ {
+ if(m_drawNodeTree) btSoftBodyHelpers::DrawNodeTree(psb,m_debugDrawer);
+ if(m_drawFaceTree) btSoftBodyHelpers::DrawFaceTree(psb,m_debugDrawer);
+ if(m_drawClusterTree) btSoftBodyHelpers::DrawClusterTree(psb,m_debugDrawer);
+ }
+ }
+ }
+}
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btSoftSoftCollisionAlgorithm.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btSoftSoftCollisionAlgorithm.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btSoftSoftCollisionAlgorithm.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btSoftSoftCollisionAlgorithm.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,46 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "BulletSoftBody/btSoftSoftCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/CollisionShapes/btBoxShape.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletSoftBody/btSoftBody.h"
+
+#define USE_PERSISTENT_CONTACTS 1
+
+btSoftSoftCollisionAlgorithm::btSoftSoftCollisionAlgorithm(btPersistentManifold* /*mf*/,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* /*obj0*/,btCollisionObject* /*obj1*/)
+: btCollisionAlgorithm(ci)
+//m_ownManifold(false),
+//m_manifoldPtr(mf)
+{
+}
+
+btSoftSoftCollisionAlgorithm::~btSoftSoftCollisionAlgorithm()
+{
+}
+
+void btSoftSoftCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/)
+{
+ btSoftBody* soft0 = (btSoftBody*)body0;
+ btSoftBody* soft1 = (btSoftBody*)body1;
+ soft0->defaultCollisionHandler(soft1);
+}
+
+btScalar btSoftSoftCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/,btCollisionObject* /*body1*/,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/)
+{
+ //not yet
+ return 1.f;
+}
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btSolve2LinearConstraint.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btSolve2LinearConstraint.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btSolve2LinearConstraint.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btSolve2LinearConstraint.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,255 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#include "BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h"
+
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btVector3.h"
+#include "BulletDynamics/ConstraintSolver/btJacobianEntry.h"
+
+
+void btSolve2LinearConstraint::resolveUnilateralPairConstraint(
+ btRigidBody* body1,
+ btRigidBody* body2,
+
+ const btMatrix3x3& world2A,
+ const btMatrix3x3& world2B,
+
+ const btVector3& invInertiaADiag,
+ const btScalar invMassA,
+ const btVector3& linvelA,const btVector3& angvelA,
+ const btVector3& rel_posA1,
+ const btVector3& invInertiaBDiag,
+ const btScalar invMassB,
+ const btVector3& linvelB,const btVector3& angvelB,
+ const btVector3& rel_posA2,
+
+ btScalar depthA, const btVector3& normalA,
+ const btVector3& rel_posB1,const btVector3& rel_posB2,
+ btScalar depthB, const btVector3& normalB,
+ btScalar& imp0,btScalar& imp1)
+{
+ (void)linvelA;
+ (void)linvelB;
+ (void)angvelB;
+ (void)angvelA;
+
+
+
+ imp0 = btScalar(0.);
+ imp1 = btScalar(0.);
+
+ btScalar len = btFabs(normalA.length()) - btScalar(1.);
+ if (btFabs(len) >= SIMD_EPSILON)
+ return;
+
+ btAssert(len < SIMD_EPSILON);
+
+
+ //this jacobian entry could be re-used for all iterations
+ btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA,
+ invInertiaBDiag,invMassB);
+ btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA,
+ invInertiaBDiag,invMassB);
+
+ //const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
+ //const btScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
+
+ const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1)-body2->getVelocityInLocalPoint(rel_posA1));
+ const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1));
+
+// btScalar penetrationImpulse = (depth*contactTau*timeCorrection) * massTerm;//jacDiagABInv
+ btScalar massTerm = btScalar(1.) / (invMassA + invMassB);
+
+
+ // calculate rhs (or error) terms
+ const btScalar dv0 = depthA * m_tau * massTerm - vel0 * m_damping;
+ const btScalar dv1 = depthB * m_tau * massTerm - vel1 * m_damping;
+
+
+ // dC/dv * dv = -C
+
+ // jacobian * impulse = -error
+ //
+
+ //impulse = jacobianInverse * -error
+
+ // inverting 2x2 symmetric system (offdiagonal are equal!)
+ //
+
+
+ btScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB);
+ btScalar invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag );
+
+ //imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
+ //imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
+
+ imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
+ imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
+
+ //[a b] [d -c]
+ //[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc)
+
+ //[jA nD] * [imp0] = [dv0]
+ //[nD jB] [imp1] [dv1]
+
+}
+
+
+
+void btSolve2LinearConstraint::resolveBilateralPairConstraint(
+ btRigidBody* body1,
+ btRigidBody* body2,
+ const btMatrix3x3& world2A,
+ const btMatrix3x3& world2B,
+
+ const btVector3& invInertiaADiag,
+ const btScalar invMassA,
+ const btVector3& linvelA,const btVector3& angvelA,
+ const btVector3& rel_posA1,
+ const btVector3& invInertiaBDiag,
+ const btScalar invMassB,
+ const btVector3& linvelB,const btVector3& angvelB,
+ const btVector3& rel_posA2,
+
+ btScalar depthA, const btVector3& normalA,
+ const btVector3& rel_posB1,const btVector3& rel_posB2,
+ btScalar depthB, const btVector3& normalB,
+ btScalar& imp0,btScalar& imp1)
+{
+
+ (void)linvelA;
+ (void)linvelB;
+ (void)angvelA;
+ (void)angvelB;
+
+
+
+ imp0 = btScalar(0.);
+ imp1 = btScalar(0.);
+
+ btScalar len = btFabs(normalA.length()) - btScalar(1.);
+ if (btFabs(len) >= SIMD_EPSILON)
+ return;
+
+ btAssert(len < SIMD_EPSILON);
+
+
+ //this jacobian entry could be re-used for all iterations
+ btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA,
+ invInertiaBDiag,invMassB);
+ btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA,
+ invInertiaBDiag,invMassB);
+
+ //const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
+ //const btScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
+
+ const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1)-body2->getVelocityInLocalPoint(rel_posA1));
+ const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1));
+
+ // calculate rhs (or error) terms
+ const btScalar dv0 = depthA * m_tau - vel0 * m_damping;
+ const btScalar dv1 = depthB * m_tau - vel1 * m_damping;
+
+ // dC/dv * dv = -C
+
+ // jacobian * impulse = -error
+ //
+
+ //impulse = jacobianInverse * -error
+
+ // inverting 2x2 symmetric system (offdiagonal are equal!)
+ //
+
+
+ btScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB);
+ btScalar invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag );
+
+ //imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
+ //imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
+
+ imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
+ imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
+
+ //[a b] [d -c]
+ //[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc)
+
+ //[jA nD] * [imp0] = [dv0]
+ //[nD jB] [imp1] [dv1]
+
+ if ( imp0 > btScalar(0.0))
+ {
+ if ( imp1 > btScalar(0.0) )
+ {
+ //both positive
+ }
+ else
+ {
+ imp1 = btScalar(0.);
+
+ // now imp0>0 imp1<0
+ imp0 = dv0 / jacA.getDiagonal();
+ if ( imp0 > btScalar(0.0) )
+ {
+ } else
+ {
+ imp0 = btScalar(0.);
+ }
+ }
+ }
+ else
+ {
+ imp0 = btScalar(0.);
+
+ imp1 = dv1 / jacB.getDiagonal();
+ if ( imp1 <= btScalar(0.0) )
+ {
+ imp1 = btScalar(0.);
+ // now imp0>0 imp1<0
+ imp0 = dv0 / jacA.getDiagonal();
+ if ( imp0 > btScalar(0.0) )
+ {
+ } else
+ {
+ imp0 = btScalar(0.);
+ }
+ } else
+ {
+ }
+ }
+}
+
+
+/*
+void btSolve2LinearConstraint::resolveAngularConstraint( const btMatrix3x3& invInertiaAWS,
+ const btScalar invMassA,
+ const btVector3& linvelA,const btVector3& angvelA,
+ const btVector3& rel_posA1,
+ const btMatrix3x3& invInertiaBWS,
+ const btScalar invMassB,
+ const btVector3& linvelB,const btVector3& angvelB,
+ const btVector3& rel_posA2,
+
+ btScalar depthA, const btVector3& normalA,
+ const btVector3& rel_posB1,const btVector3& rel_posB2,
+ btScalar depthB, const btVector3& normalB,
+ btScalar& imp0,btScalar& imp1)
+{
+
+}
+*/
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btSphereBoxCollisionAlgorithm.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btSphereBoxCollisionAlgorithm.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btSphereBoxCollisionAlgorithm.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btSphereBoxCollisionAlgorithm.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,260 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+#include "BulletCollision/CollisionShapes/btBoxShape.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+//#include <stdio.h>
+
+btSphereBoxCollisionAlgorithm::btSphereBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped)
+: btActivatingCollisionAlgorithm(ci,col0,col1),
+m_ownManifold(false),
+m_manifoldPtr(mf),
+m_isSwapped(isSwapped)
+{
+ btCollisionObject* sphereObj = m_isSwapped? col1 : col0;
+ btCollisionObject* boxObj = m_isSwapped? col0 : col1;
+
+ if (!m_manifoldPtr && m_dispatcher->needsCollision(sphereObj,boxObj))
+ {
+ m_manifoldPtr = m_dispatcher->getNewManifold(sphereObj,boxObj);
+ m_ownManifold = true;
+ }
+}
+
+
+btSphereBoxCollisionAlgorithm::~btSphereBoxCollisionAlgorithm()
+{
+ if (m_ownManifold)
+ {
+ if (m_manifoldPtr)
+ m_dispatcher->releaseManifold(m_manifoldPtr);
+ }
+}
+
+
+
+void btSphereBoxCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+ (void)dispatchInfo;
+ (void)resultOut;
+ if (!m_manifoldPtr)
+ return;
+
+ btCollisionObject* sphereObj = m_isSwapped? body1 : body0;
+ btCollisionObject* boxObj = m_isSwapped? body0 : body1;
+
+
+ btSphereShape* sphere0 = (btSphereShape*)sphereObj->getCollisionShape();
+
+ btVector3 normalOnSurfaceB;
+ btVector3 pOnBox,pOnSphere;
+ btVector3 sphereCenter = sphereObj->getWorldTransform().getOrigin();
+ btScalar radius = sphere0->getRadius();
+
+ btScalar dist = getSphereDistance(boxObj,pOnBox,pOnSphere,sphereCenter,radius);
+
+ resultOut->setPersistentManifold(m_manifoldPtr);
+
+ if (dist < SIMD_EPSILON)
+ {
+ btVector3 normalOnSurfaceB = (pOnBox- pOnSphere).normalize();
+
+ /// report a contact. internally this will be kept persistent, and contact reduction is done
+
+ resultOut->addContactPoint(normalOnSurfaceB,pOnBox,dist);
+
+ }
+
+ if (m_ownManifold)
+ {
+ if (m_manifoldPtr->getNumContacts())
+ {
+ resultOut->refreshContactPoints();
+ }
+ }
+
+}
+
+btScalar btSphereBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+ (void)resultOut;
+ (void)dispatchInfo;
+ (void)col0;
+ (void)col1;
+
+ //not yet
+ return btScalar(1.);
+}
+
+
+btScalar btSphereBoxCollisionAlgorithm::getSphereDistance(btCollisionObject* boxObj, btVector3& pointOnBox, btVector3& v3PointOnSphere, const btVector3& sphereCenter, btScalar fRadius )
+{
+
+ btScalar margins;
+ btVector3 bounds[2];
+ btBoxShape* boxShape= (btBoxShape*)boxObj->getCollisionShape();
+
+ bounds[0] = -boxShape->getHalfExtentsWithoutMargin();
+ bounds[1] = boxShape->getHalfExtentsWithoutMargin();
+
+ margins = boxShape->getMargin();//also add sphereShape margin?
+
+ const btTransform& m44T = boxObj->getWorldTransform();
+
+ btVector3 boundsVec[2];
+ btScalar fPenetration;
+
+ boundsVec[0] = bounds[0];
+ boundsVec[1] = bounds[1];
+
+ btVector3 marginsVec( margins, margins, margins );
+
+ // add margins
+ bounds[0] += marginsVec;
+ bounds[1] -= marginsVec;
+
+ /////////////////////////////////////////////////
+
+ btVector3 tmp, prel, n[6], normal, v3P;
+ btScalar fSep = btScalar(10000000.0), fSepThis;
+
+ n[0].setValue( btScalar(-1.0), btScalar(0.0), btScalar(0.0) );
+ n[1].setValue( btScalar(0.0), btScalar(-1.0), btScalar(0.0) );
+ n[2].setValue( btScalar(0.0), btScalar(0.0), btScalar(-1.0) );
+ n[3].setValue( btScalar(1.0), btScalar(0.0), btScalar(0.0) );
+ n[4].setValue( btScalar(0.0), btScalar(1.0), btScalar(0.0) );
+ n[5].setValue( btScalar(0.0), btScalar(0.0), btScalar(1.0) );
+
+ // convert point in local space
+ prel = m44T.invXform( sphereCenter);
+
+ bool bFound = false;
+
+ v3P = prel;
+
+ for (int i=0;i<6;i++)
+ {
+ int j = i<3? 0:1;
+ if ( (fSepThis = ((v3P-bounds[j]) .dot(n[i]))) > btScalar(0.0) )
+ {
+ v3P = v3P - n[i]*fSepThis;
+ bFound = true;
+ }
+ }
+
+ //
+
+ if ( bFound )
+ {
+ bounds[0] = boundsVec[0];
+ bounds[1] = boundsVec[1];
+
+ normal = (prel - v3P).normalize();
+ pointOnBox = v3P + normal*margins;
+ v3PointOnSphere = prel - normal*fRadius;
+
+ if ( ((v3PointOnSphere - pointOnBox) .dot (normal)) > btScalar(0.0) )
+ {
+ return btScalar(1.0);
+ }
+
+ // transform back in world space
+ tmp = m44T( pointOnBox);
+ pointOnBox = tmp;
+ tmp = m44T( v3PointOnSphere);
+ v3PointOnSphere = tmp;
+ btScalar fSeps2 = (pointOnBox-v3PointOnSphere).length2();
+
+ //if this fails, fallback into deeper penetration case, below
+ if (fSeps2 > SIMD_EPSILON)
+ {
+ fSep = - btSqrt(fSeps2);
+ normal = (pointOnBox-v3PointOnSphere);
+ normal *= btScalar(1.)/fSep;
+ }
+
+ return fSep;
+ }
+
+ //////////////////////////////////////////////////
+ // Deep penetration case
+
+ fPenetration = getSpherePenetration( boxObj,pointOnBox, v3PointOnSphere, sphereCenter, fRadius,bounds[0],bounds[1] );
+
+ bounds[0] = boundsVec[0];
+ bounds[1] = boundsVec[1];
+
+ if ( fPenetration <= btScalar(0.0) )
+ return (fPenetration-margins);
+ else
+ return btScalar(1.0);
+}
+
+btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btCollisionObject* boxObj,btVector3& pointOnBox, btVector3& v3PointOnSphere, const btVector3& sphereCenter, btScalar fRadius, const btVector3& aabbMin, const btVector3& aabbMax)
+{
+
+ btVector3 bounds[2];
+
+ bounds[0] = aabbMin;
+ bounds[1] = aabbMax;
+
+ btVector3 p0, tmp, prel, n[6], normal;
+ btScalar fSep = btScalar(-10000000.0), fSepThis;
+
+ // set p0 and normal to a default value to shup up GCC
+ p0.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
+ normal.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
+
+ n[0].setValue( btScalar(-1.0), btScalar(0.0), btScalar(0.0) );
+ n[1].setValue( btScalar(0.0), btScalar(-1.0), btScalar(0.0) );
+ n[2].setValue( btScalar(0.0), btScalar(0.0), btScalar(-1.0) );
+ n[3].setValue( btScalar(1.0), btScalar(0.0), btScalar(0.0) );
+ n[4].setValue( btScalar(0.0), btScalar(1.0), btScalar(0.0) );
+ n[5].setValue( btScalar(0.0), btScalar(0.0), btScalar(1.0) );
+
+ const btTransform& m44T = boxObj->getWorldTransform();
+
+ // convert point in local space
+ prel = m44T.invXform( sphereCenter);
+
+ ///////////
+
+ for (int i=0;i<6;i++)
+ {
+ int j = i<3 ? 0:1;
+ if ( (fSepThis = ((prel-bounds[j]) .dot( n[i]))-fRadius) > btScalar(0.0) ) return btScalar(1.0);
+ if ( fSepThis > fSep )
+ {
+ p0 = bounds[j]; normal = (btVector3&)n[i];
+ fSep = fSepThis;
+ }
+ }
+
+ pointOnBox = prel - normal*(normal.dot((prel-p0)));
+ v3PointOnSphere = pointOnBox + normal*fSep;
+
+ // transform back in world space
+ tmp = m44T( pointOnBox);
+ pointOnBox = tmp;
+ tmp = m44T( v3PointOnSphere); v3PointOnSphere = tmp;
+ normal = (pointOnBox-v3PointOnSphere).normalize();
+
+ return fSep;
+
+}
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btSphereShape.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btSphereShape.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btSphereShape.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btSphereShape.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,71 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+#include "BulletCollision/CollisionShapes/btCollisionMargin.h"
+
+#include "LinearMath/btQuaternion.h"
+
+btVector3 btSphereShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const
+{
+ (void)vec;
+ return btVector3(btScalar(0.),btScalar(0.),btScalar(0.));
+}
+
+void btSphereShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
+{
+ (void)vectors;
+
+ for (int i=0;i<numVectors;i++)
+ {
+ supportVerticesOut[i].setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+ }
+}
+
+
+btVector3 btSphereShape::localGetSupportingVertex(const btVector3& vec)const
+{
+ btVector3 supVertex;
+ supVertex = localGetSupportingVertexWithoutMargin(vec);
+
+ btVector3 vecnorm = vec;
+ if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON))
+ {
+ vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.));
+ }
+ vecnorm.normalize();
+ supVertex+= getMargin() * vecnorm;
+ return supVertex;
+}
+
+
+//broken due to scaling
+void btSphereShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
+{
+ const btVector3& center = t.getOrigin();
+ btVector3 extent(getMargin(),getMargin(),getMargin());
+ aabbMin = center - extent;
+ aabbMax = center + extent;
+}
+
+
+
+void btSphereShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+{
+ btScalar elem = btScalar(0.4) * mass * getMargin()*getMargin();
+ inertia.setValue(elem,elem,elem);
+
+}
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btSphereSphereCollisionAlgorithm.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btSphereSphereCollisionAlgorithm.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btSphereSphereCollisionAlgorithm.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btSphereSphereCollisionAlgorithm.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,105 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+
+btSphereSphereCollisionAlgorithm::btSphereSphereCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1)
+: btActivatingCollisionAlgorithm(ci,col0,col1),
+m_ownManifold(false),
+m_manifoldPtr(mf)
+{
+ if (!m_manifoldPtr)
+ {
+ m_manifoldPtr = m_dispatcher->getNewManifold(col0,col1);
+ m_ownManifold = true;
+ }
+}
+
+btSphereSphereCollisionAlgorithm::~btSphereSphereCollisionAlgorithm()
+{
+ if (m_ownManifold)
+ {
+ if (m_manifoldPtr)
+ m_dispatcher->releaseManifold(m_manifoldPtr);
+ }
+}
+
+void btSphereSphereCollisionAlgorithm::processCollision (btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+ (void)dispatchInfo;
+
+ if (!m_manifoldPtr)
+ return;
+
+ resultOut->setPersistentManifold(m_manifoldPtr);
+
+ btSphereShape* sphere0 = (btSphereShape*)col0->getCollisionShape();
+ btSphereShape* sphere1 = (btSphereShape*)col1->getCollisionShape();
+
+ btVector3 diff = col0->getWorldTransform().getOrigin()- col1->getWorldTransform().getOrigin();
+ btScalar len = diff.length();
+ btScalar radius0 = sphere0->getRadius();
+ btScalar radius1 = sphere1->getRadius();
+
+#ifdef CLEAR_MANIFOLD
+ m_manifoldPtr->clearManifold(); //don't do this, it disables warmstarting
+#endif
+
+ ///iff distance positive, don't generate a new contact
+ if ( len > (radius0+radius1))
+ {
+#ifndef CLEAR_MANIFOLD
+ resultOut->refreshContactPoints();
+#endif //CLEAR_MANIFOLD
+ return;
+ }
+ ///distance (negative means penetration)
+ btScalar dist = len - (radius0+radius1);
+
+ btVector3 normalOnSurfaceB(1,0,0);
+ if (len > SIMD_EPSILON)
+ {
+ normalOnSurfaceB = diff / len;
+ }
+
+ ///point on A (worldspace)
+ ///btVector3 pos0 = col0->getWorldTransform().getOrigin() - radius0 * normalOnSurfaceB;
+ ///point on B (worldspace)
+ btVector3 pos1 = col1->getWorldTransform().getOrigin() + radius1* normalOnSurfaceB;
+
+ /// report a contact. internally this will be kept persistent, and contact reduction is done
+
+
+ resultOut->addContactPoint(normalOnSurfaceB,pos1,dist);
+
+#ifndef CLEAR_MANIFOLD
+ resultOut->refreshContactPoints();
+#endif //CLEAR_MANIFOLD
+
+}
+
+btScalar btSphereSphereCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+ (void)col0;
+ (void)col1;
+ (void)dispatchInfo;
+ (void)resultOut;
+
+ //not yet
+ return btScalar(1.);
+}
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btSphereTriangleCollisionAlgorithm.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btSphereTriangleCollisionAlgorithm.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btSphereTriangleCollisionAlgorithm.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btSphereTriangleCollisionAlgorithm.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,84 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionDispatch/SphereTriangleDetector.h"
+
+
+btSphereTriangleCollisionAlgorithm::btSphereTriangleCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1,bool swapped)
+: btActivatingCollisionAlgorithm(ci,col0,col1),
+m_ownManifold(false),
+m_manifoldPtr(mf),
+m_swapped(swapped)
+{
+ if (!m_manifoldPtr)
+ {
+ m_manifoldPtr = m_dispatcher->getNewManifold(col0,col1);
+ m_ownManifold = true;
+ }
+}
+
+btSphereTriangleCollisionAlgorithm::~btSphereTriangleCollisionAlgorithm()
+{
+ if (m_ownManifold)
+ {
+ if (m_manifoldPtr)
+ m_dispatcher->releaseManifold(m_manifoldPtr);
+ }
+}
+
+void btSphereTriangleCollisionAlgorithm::processCollision (btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+ if (!m_manifoldPtr)
+ return;
+
+ btCollisionObject* sphereObj = m_swapped? col1 : col0;
+ btCollisionObject* triObj = m_swapped? col0 : col1;
+
+ btSphereShape* sphere = (btSphereShape*)sphereObj->getCollisionShape();
+ btTriangleShape* triangle = (btTriangleShape*)triObj->getCollisionShape();
+
+ /// report a contact. internally this will be kept persistent, and contact reduction is done
+ resultOut->setPersistentManifold(m_manifoldPtr);
+ SphereTriangleDetector detector(sphere,triangle, m_manifoldPtr->getContactBreakingThreshold());
+
+ btDiscreteCollisionDetectorInterface::ClosestPointInput input;
+ input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);///@todo: tighter bounds
+ input.m_transformA = sphereObj->getWorldTransform();
+ input.m_transformB = triObj->getWorldTransform();
+
+ bool swapResults = m_swapped;
+
+ detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw,swapResults);
+
+ if (m_ownManifold)
+ resultOut->refreshContactPoints();
+
+}
+
+btScalar btSphereTriangleCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+ (void)resultOut;
+ (void)dispatchInfo;
+ (void)col0;
+ (void)col1;
+
+ //not yet
+ return btScalar(1.);
+}
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btStaticPlaneShape.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btStaticPlaneShape.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btStaticPlaneShape.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btStaticPlaneShape.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,107 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
+
+#include "LinearMath/btTransformUtil.h"
+
+
+btStaticPlaneShape::btStaticPlaneShape(const btVector3& planeNormal,btScalar planeConstant)
+: btConcaveShape (), m_planeNormal(planeNormal.normalized()),
+m_planeConstant(planeConstant),
+m_localScaling(btScalar(0.),btScalar(0.),btScalar(0.))
+{
+ m_shapeType = STATIC_PLANE_PROXYTYPE;
+ // btAssert( btFuzzyZero(m_planeNormal.length() - btScalar(1.)) );
+}
+
+
+btStaticPlaneShape::~btStaticPlaneShape()
+{
+}
+
+
+
+void btStaticPlaneShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
+{
+ (void)t;
+ /*
+ btVector3 infvec (btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
+
+ btVector3 center = m_planeNormal*m_planeConstant;
+ aabbMin = center + infvec*m_planeNormal;
+ aabbMax = aabbMin;
+ aabbMin.setMin(center - infvec*m_planeNormal);
+ aabbMax.setMax(center - infvec*m_planeNormal);
+ */
+
+ aabbMin.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
+ aabbMax.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
+
+}
+
+
+
+
+void btStaticPlaneShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
+{
+
+ btVector3 halfExtents = (aabbMax - aabbMin) * btScalar(0.5);
+ btScalar radius = halfExtents.length();
+ btVector3 center = (aabbMax + aabbMin) * btScalar(0.5);
+
+ //this is where the triangles are generated, given AABB and plane equation (normal/constant)
+
+ btVector3 tangentDir0,tangentDir1;
+
+ //tangentDir0/tangentDir1 can be precalculated
+ btPlaneSpace1(m_planeNormal,tangentDir0,tangentDir1);
+
+ btVector3 supVertex0,supVertex1;
+
+ btVector3 projectedCenter = center - (m_planeNormal.dot(center) - m_planeConstant)*m_planeNormal;
+
+ btVector3 triangle[3];
+ triangle[0] = projectedCenter + tangentDir0*radius + tangentDir1*radius;
+ triangle[1] = projectedCenter + tangentDir0*radius - tangentDir1*radius;
+ triangle[2] = projectedCenter - tangentDir0*radius - tangentDir1*radius;
+
+ callback->processTriangle(triangle,0,0);
+
+ triangle[0] = projectedCenter - tangentDir0*radius - tangentDir1*radius;
+ triangle[1] = projectedCenter - tangentDir0*radius + tangentDir1*radius;
+ triangle[2] = projectedCenter + tangentDir0*radius + tangentDir1*radius;
+
+ callback->processTriangle(triangle,0,1);
+
+}
+
+void btStaticPlaneShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+{
+ (void)mass;
+
+ //moving concave objects not supported
+
+ inertia.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+}
+
+void btStaticPlaneShape::setLocalScaling(const btVector3& scaling)
+{
+ m_localScaling = scaling;
+}
+const btVector3& btStaticPlaneShape::getLocalScaling() const
+{
+ return m_localScaling;
+}
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btStridingMeshInterface.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btStridingMeshInterface.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btStridingMeshInterface.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btStridingMeshInterface.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,181 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "BulletCollision/CollisionShapes/btStridingMeshInterface.h"
+
+btStridingMeshInterface::~btStridingMeshInterface()
+{
+
+}
+
+
+void btStridingMeshInterface::InternalProcessAllTriangles(btInternalTriangleIndexCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
+{
+ (void)aabbMin;
+ (void)aabbMax;
+ int numtotalphysicsverts = 0;
+ int part,graphicssubparts = getNumSubParts();
+ const unsigned char * vertexbase;
+ const unsigned char * indexbase;
+ int indexstride;
+ PHY_ScalarType type;
+ PHY_ScalarType gfxindextype;
+ int stride,numverts,numtriangles;
+ int gfxindex;
+ btVector3 triangle[3];
+
+ btVector3 meshScaling = getScaling();
+
+ ///if the number of parts is big, the performance might drop due to the innerloop switch on indextype
+ for (part=0;part<graphicssubparts ;part++)
+ {
+ getLockedReadOnlyVertexIndexBase(&vertexbase,numverts,type,stride,&indexbase,indexstride,numtriangles,gfxindextype,part);
+ numtotalphysicsverts+=numtriangles*3; //upper bound
+
+ ///unlike that developers want to pass in double-precision meshes in single-precision Bullet build
+ ///so disable this feature by default
+ ///see patch http://code.google.com/p/bullet/issues/detail?id=213
+
+ switch (type)
+ {
+ case PHY_FLOAT:
+ {
+
+ float* graphicsbase;
+
+ switch (gfxindextype)
+ {
+ case PHY_INTEGER:
+ {
+ for (gfxindex=0;gfxindex<numtriangles;gfxindex++)
+ {
+ unsigned int* tri_indices= (unsigned int*)(indexbase+gfxindex*indexstride);
+ graphicsbase = (float*)(vertexbase+tri_indices[0]*stride);
+ triangle[0].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ());
+ graphicsbase = (float*)(vertexbase+tri_indices[1]*stride);
+ triangle[1].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ());
+ graphicsbase = (float*)(vertexbase+tri_indices[2]*stride);
+ triangle[2].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ());
+ callback->internalProcessTriangleIndex(triangle,part,gfxindex);
+ }
+ break;
+ }
+ case PHY_SHORT:
+ {
+ for (gfxindex=0;gfxindex<numtriangles;gfxindex++)
+ {
+ unsigned short int* tri_indices= (unsigned short int*)(indexbase+gfxindex*indexstride);
+ graphicsbase = (float*)(vertexbase+tri_indices[0]*stride);
+ triangle[0].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ());
+ graphicsbase = (float*)(vertexbase+tri_indices[1]*stride);
+ triangle[1].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ());
+ graphicsbase = (float*)(vertexbase+tri_indices[2]*stride);
+ triangle[2].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ());
+ callback->internalProcessTriangleIndex(triangle,part,gfxindex);
+ }
+ break;
+ }
+ default:
+ btAssert((gfxindextype == PHY_INTEGER) || (gfxindextype == PHY_SHORT));
+ }
+ break;
+ }
+
+ case PHY_DOUBLE:
+ {
+ double* graphicsbase;
+
+ switch (gfxindextype)
+ {
+ case PHY_INTEGER:
+ {
+ for (gfxindex=0;gfxindex<numtriangles;gfxindex++)
+ {
+ unsigned int* tri_indices= (unsigned int*)(indexbase+gfxindex*indexstride);
+ graphicsbase = (double*)(vertexbase+tri_indices[0]*stride);
+ triangle[0].setValue((btScalar)graphicsbase[0]*meshScaling.getX(),(btScalar)graphicsbase[1]*meshScaling.getY(),(btScalar)graphicsbase[2]*meshScaling.getZ());
+ graphicsbase = (double*)(vertexbase+tri_indices[1]*stride);
+ triangle[1].setValue((btScalar)graphicsbase[0]*meshScaling.getX(),(btScalar)graphicsbase[1]*meshScaling.getY(), (btScalar)graphicsbase[2]*meshScaling.getZ());
+ graphicsbase = (double*)(vertexbase+tri_indices[2]*stride);
+ triangle[2].setValue((btScalar)graphicsbase[0]*meshScaling.getX(),(btScalar)graphicsbase[1]*meshScaling.getY(), (btScalar)graphicsbase[2]*meshScaling.getZ());
+ callback->internalProcessTriangleIndex(triangle,part,gfxindex);
+ }
+ break;
+ }
+ case PHY_SHORT:
+ {
+ for (gfxindex=0;gfxindex<numtriangles;gfxindex++)
+ {
+ unsigned short int* tri_indices= (unsigned short int*)(indexbase+gfxindex*indexstride);
+ graphicsbase = (double*)(vertexbase+tri_indices[0]*stride);
+ triangle[0].setValue((btScalar)graphicsbase[0]*meshScaling.getX(),(btScalar)graphicsbase[1]*meshScaling.getY(),(btScalar)graphicsbase[2]*meshScaling.getZ());
+ graphicsbase = (double*)(vertexbase+tri_indices[1]*stride);
+ triangle[1].setValue((btScalar)graphicsbase[0]*meshScaling.getX(),(btScalar)graphicsbase[1]*meshScaling.getY(), (btScalar)graphicsbase[2]*meshScaling.getZ());
+ graphicsbase = (double*)(vertexbase+tri_indices[2]*stride);
+ triangle[2].setValue((btScalar)graphicsbase[0]*meshScaling.getX(),(btScalar)graphicsbase[1]*meshScaling.getY(), (btScalar)graphicsbase[2]*meshScaling.getZ());
+ callback->internalProcessTriangleIndex(triangle,part,gfxindex);
+ }
+ break;
+ }
+ default:
+ btAssert((gfxindextype == PHY_INTEGER) || (gfxindextype == PHY_SHORT));
+ }
+ break;
+ }
+ default:
+ btAssert((type == PHY_FLOAT) || (type == PHY_DOUBLE));
+ }
+
+ unLockReadOnlyVertexBase(part);
+ }
+}
+
+void btStridingMeshInterface::calculateAabbBruteForce(btVector3& aabbMin,btVector3& aabbMax)
+{
+
+ struct AabbCalculationCallback : public btInternalTriangleIndexCallback
+ {
+ btVector3 m_aabbMin;
+ btVector3 m_aabbMax;
+
+ AabbCalculationCallback()
+ {
+ m_aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
+ m_aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
+ }
+
+ virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex)
+ {
+ (void)partId;
+ (void)triangleIndex;
+
+ m_aabbMin.setMin(triangle[0]);
+ m_aabbMax.setMax(triangle[0]);
+ m_aabbMin.setMin(triangle[1]);
+ m_aabbMax.setMax(triangle[1]);
+ m_aabbMin.setMin(triangle[2]);
+ m_aabbMax.setMax(triangle[2]);
+ }
+ };
+
+ //first calculate the total aabb for all triangles
+ AabbCalculationCallback aabbCallback;
+ aabbMin.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
+ aabbMax.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
+ InternalProcessAllTriangles(&aabbCallback,aabbMin,aabbMax);
+
+ aabbMin = aabbCallback.m_aabbMin;
+ aabbMax = aabbCallback.m_aabbMax;
+}
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btSubSimplexConvexCast.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btSubSimplexConvexCast.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btSubSimplexConvexCast.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btSubSimplexConvexCast.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,157 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+
+#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h"
+#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
+#include "BulletCollision/NarrowPhaseCollision/btPointCollector.h"
+#include "LinearMath/btTransformUtil.h"
+
+btSubsimplexConvexCast::btSubsimplexConvexCast (const btConvexShape* convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver)
+:m_simplexSolver(simplexSolver),
+m_convexA(convexA),m_convexB(convexB)
+{
+}
+
+///Typically the conservative advancement reaches solution in a few iterations, clip it to 32 for degenerate cases.
+///See discussion about this here http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=565
+#ifdef BT_USE_DOUBLE_PRECISION
+#define MAX_ITERATIONS 64
+#else
+#define MAX_ITERATIONS 32
+#endif
+bool btSubsimplexConvexCast::calcTimeOfImpact(
+ const btTransform& fromA,
+ const btTransform& toA,
+ const btTransform& fromB,
+ const btTransform& toB,
+ CastResult& result)
+{
+
+ m_simplexSolver->reset();
+
+ btVector3 linVelA,linVelB;
+ linVelA = toA.getOrigin()-fromA.getOrigin();
+ linVelB = toB.getOrigin()-fromB.getOrigin();
+
+ btScalar lambda = btScalar(0.);
+
+ btTransform interpolatedTransA = fromA;
+ btTransform interpolatedTransB = fromB;
+
+ ///take relative motion
+ btVector3 r = (linVelA-linVelB);
+ btVector3 v;
+
+ btVector3 supVertexA = fromA(m_convexA->localGetSupportingVertex(-r*fromA.getBasis()));
+ btVector3 supVertexB = fromB(m_convexB->localGetSupportingVertex(r*fromB.getBasis()));
+ v = supVertexA-supVertexB;
+ int maxIter = MAX_ITERATIONS;
+
+ btVector3 n;
+ n.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+ bool hasResult = false;
+ btVector3 c;
+
+ btScalar lastLambda = lambda;
+
+
+ btScalar dist2 = v.length2();
+#ifdef BT_USE_DOUBLE_PRECISION
+ btScalar epsilon = btScalar(0.0001);
+#else
+ btScalar epsilon = btScalar(0.0001);
+#endif //BT_USE_DOUBLE_PRECISION
+ btVector3 w,p;
+ btScalar VdotR;
+
+ while ( (dist2 > epsilon) && maxIter--)
+ {
+ supVertexA = interpolatedTransA(m_convexA->localGetSupportingVertex(-v*interpolatedTransA.getBasis()));
+ supVertexB = interpolatedTransB(m_convexB->localGetSupportingVertex(v*interpolatedTransB.getBasis()));
+ w = supVertexA-supVertexB;
+
+ btScalar VdotW = v.dot(w);
+
+ if (lambda > btScalar(1.0))
+ {
+ return false;
+ }
+
+ if ( VdotW > btScalar(0.))
+ {
+ VdotR = v.dot(r);
+
+ if (VdotR >= -(SIMD_EPSILON*SIMD_EPSILON))
+ return false;
+ else
+ {
+ lambda = lambda - VdotW / VdotR;
+ //interpolate to next lambda
+ // x = s + lambda * r;
+ interpolatedTransA.getOrigin().setInterpolate3(fromA.getOrigin(),toA.getOrigin(),lambda);
+ interpolatedTransB.getOrigin().setInterpolate3(fromB.getOrigin(),toB.getOrigin(),lambda);
+ //m_simplexSolver->reset();
+ //check next line
+ w = supVertexA-supVertexB;
+ lastLambda = lambda;
+ n = v;
+ hasResult = true;
+ }
+ }
+ m_simplexSolver->addVertex( w, supVertexA , supVertexB);
+ if (m_simplexSolver->closest(v))
+ {
+ dist2 = v.length2();
+ hasResult = true;
+ //todo: check this normal for validity
+ //n=v;
+ //printf("V=%f , %f, %f\n",v[0],v[1],v[2]);
+ //printf("DIST2=%f\n",dist2);
+ //printf("numverts = %i\n",m_simplexSolver->numVertices());
+ } else
+ {
+ dist2 = btScalar(0.);
+ }
+ }
+
+ //int numiter = MAX_ITERATIONS - maxIter;
+// printf("number of iterations: %d", numiter);
+
+ //don't report a time of impact when moving 'away' from the hitnormal
+
+
+ result.m_fraction = lambda;
+ if (n.length2() >= (SIMD_EPSILON*SIMD_EPSILON))
+ result.m_normal = n.normalized();
+ else
+ result.m_normal = btVector3(btScalar(0.0), btScalar(0.0), btScalar(0.0));
+
+ //don't report time of impact for motion away from the contact normal (or causes minor penetration)
+ if (result.m_normal.dot(r)>=-result.m_allowedPenetration)
+ return false;
+
+ btVector3 hitA,hitB;
+ m_simplexSolver->compute_points(hitA,hitB);
+ result.m_hitPoint=hitB;
+ return true;
+}
+
+
+
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btTetrahedronShape.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btTetrahedronShape.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btTetrahedronShape.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btTetrahedronShape.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,218 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "BulletCollision/CollisionShapes/btTetrahedronShape.h"
+#include "LinearMath/btMatrix3x3.h"
+
+btBU_Simplex1to4::btBU_Simplex1to4() : btPolyhedralConvexAabbCachingShape (),
+m_numVertices(0)
+{
+ m_shapeType = TETRAHEDRAL_SHAPE_PROXYTYPE;
+}
+
+btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0) : btPolyhedralConvexAabbCachingShape (),
+m_numVertices(0)
+{
+ m_shapeType = TETRAHEDRAL_SHAPE_PROXYTYPE;
+ addVertex(pt0);
+}
+
+btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0,const btVector3& pt1) : btPolyhedralConvexAabbCachingShape (),
+m_numVertices(0)
+{
+ m_shapeType = TETRAHEDRAL_SHAPE_PROXYTYPE;
+ addVertex(pt0);
+ addVertex(pt1);
+}
+
+btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0,const btVector3& pt1,const btVector3& pt2) : btPolyhedralConvexAabbCachingShape (),
+m_numVertices(0)
+{
+ m_shapeType = TETRAHEDRAL_SHAPE_PROXYTYPE;
+ addVertex(pt0);
+ addVertex(pt1);
+ addVertex(pt2);
+}
+
+btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0,const btVector3& pt1,const btVector3& pt2,const btVector3& pt3) : btPolyhedralConvexAabbCachingShape (),
+m_numVertices(0)
+{
+ m_shapeType = TETRAHEDRAL_SHAPE_PROXYTYPE;
+ addVertex(pt0);
+ addVertex(pt1);
+ addVertex(pt2);
+ addVertex(pt3);
+}
+
+
+void btBU_Simplex1to4::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
+{
+#if 1
+ btPolyhedralConvexAabbCachingShape::getAabb(t,aabbMin,aabbMax);
+#else
+ aabbMin.setValue(BT_LARGE_FLOAT,BT_LARGE_FLOAT,BT_LARGE_FLOAT);
+ aabbMax.setValue(-BT_LARGE_FLOAT,-BT_LARGE_FLOAT,-BT_LARGE_FLOAT);
+
+ //just transform the vertices in worldspace, and take their AABB
+ for (int i=0;i<m_numVertices;i++)
+ {
+ btVector3 worldVertex = t(m_vertices[i]);
+ aabbMin.setMin(worldVertex);
+ aabbMax.setMax(worldVertex);
+ }
+#endif
+}
+
+
+
+
+
+void btBU_Simplex1to4::addVertex(const btVector3& pt)
+{
+ m_vertices[m_numVertices++] = pt;
+ recalcLocalAabb();
+}
+
+
+int btBU_Simplex1to4::getNumVertices() const
+{
+ return m_numVertices;
+}
+
+int btBU_Simplex1to4::getNumEdges() const
+{
+ //euler formula, F-E+V = 2, so E = F+V-2
+
+ switch (m_numVertices)
+ {
+ case 0:
+ return 0;
+ case 1: return 0;
+ case 2: return 1;
+ case 3: return 3;
+ case 4: return 6;
+
+
+ }
+
+ return 0;
+}
+
+void btBU_Simplex1to4::getEdge(int i,btVector3& pa,btVector3& pb) const
+{
+
+ switch (m_numVertices)
+ {
+
+ case 2:
+ pa = m_vertices[0];
+ pb = m_vertices[1];
+ break;
+ case 3:
+ switch (i)
+ {
+ case 0:
+ pa = m_vertices[0];
+ pb = m_vertices[1];
+ break;
+ case 1:
+ pa = m_vertices[1];
+ pb = m_vertices[2];
+ break;
+ case 2:
+ pa = m_vertices[2];
+ pb = m_vertices[0];
+ break;
+
+ }
+ break;
+ case 4:
+ switch (i)
+ {
+ case 0:
+ pa = m_vertices[0];
+ pb = m_vertices[1];
+ break;
+ case 1:
+ pa = m_vertices[1];
+ pb = m_vertices[2];
+ break;
+ case 2:
+ pa = m_vertices[2];
+ pb = m_vertices[0];
+ break;
+ case 3:
+ pa = m_vertices[0];
+ pb = m_vertices[3];
+ break;
+ case 4:
+ pa = m_vertices[1];
+ pb = m_vertices[3];
+ break;
+ case 5:
+ pa = m_vertices[2];
+ pb = m_vertices[3];
+ break;
+ }
+
+ }
+
+
+
+
+}
+
+void btBU_Simplex1to4::getVertex(int i,btVector3& vtx) const
+{
+ vtx = m_vertices[i];
+}
+
+int btBU_Simplex1to4::getNumPlanes() const
+{
+ switch (m_numVertices)
+ {
+ case 0:
+ return 0;
+ case 1:
+ return 0;
+ case 2:
+ return 0;
+ case 3:
+ return 2;
+ case 4:
+ return 4;
+ default:
+ {
+ }
+ }
+ return 0;
+}
+
+
+void btBU_Simplex1to4::getPlane(btVector3&, btVector3& ,int ) const
+{
+
+}
+
+int btBU_Simplex1to4::getIndex(int ) const
+{
+ return 0;
+}
+
+bool btBU_Simplex1to4::isInside(const btVector3& ,btScalar ) const
+{
+ return false;
+}
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btTriangleBuffer.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btTriangleBuffer.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btTriangleBuffer.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btTriangleBuffer.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,35 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "BulletCollision/CollisionShapes/btTriangleBuffer.h"
+
+
+
+
+
+
+
+void btTriangleBuffer::processTriangle(btVector3* triangle,int partId,int triangleIndex)
+{
+ btTriangle tri;
+ tri.m_vertex0 = triangle[0];
+ tri.m_vertex1 = triangle[1];
+ tri.m_vertex2 = triangle[2];
+ tri.m_partId = partId;
+ tri.m_triangleIndex = triangleIndex;
+
+ m_triangleBuffer.push_back(tri);
+}
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btTriangleCallback.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btTriangleCallback.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btTriangleCallback.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btTriangleCallback.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,28 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "BulletCollision/CollisionShapes/btTriangleCallback.h"
+
+btTriangleCallback::~btTriangleCallback()
+{
+
+}
+
+
+btInternalTriangleIndexCallback::~btInternalTriangleIndexCallback()
+{
+
+}
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btTriangleIndexVertexArray.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btTriangleIndexVertexArray.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btTriangleIndexVertexArray.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btTriangleIndexVertexArray.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,95 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h"
+
+btTriangleIndexVertexArray::btTriangleIndexVertexArray(int numTriangles,int* triangleIndexBase,int triangleIndexStride,int numVertices,btScalar* vertexBase,int vertexStride)
+: m_hasAabb(0)
+{
+ btIndexedMesh mesh;
+
+ mesh.m_numTriangles = numTriangles;
+ mesh.m_triangleIndexBase = (const unsigned char *)triangleIndexBase;
+ mesh.m_triangleIndexStride = triangleIndexStride;
+ mesh.m_numVertices = numVertices;
+ mesh.m_vertexBase = (const unsigned char *)vertexBase;
+ mesh.m_vertexStride = vertexStride;
+
+ addIndexedMesh(mesh);
+
+}
+
+btTriangleIndexVertexArray::~btTriangleIndexVertexArray()
+{
+
+}
+
+void btTriangleIndexVertexArray::getLockedVertexIndexBase(unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& vertexStride,unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart)
+{
+ btAssert(subpart< getNumSubParts() );
+
+ btIndexedMesh& mesh = m_indexedMeshes[subpart];
+
+ numverts = mesh.m_numVertices;
+ (*vertexbase) = (unsigned char *) mesh.m_vertexBase;
+
+ type = mesh.m_vertexType;
+
+ vertexStride = mesh.m_vertexStride;
+
+ numfaces = mesh.m_numTriangles;
+
+ (*indexbase) = (unsigned char *)mesh.m_triangleIndexBase;
+ indexstride = mesh.m_triangleIndexStride;
+ indicestype = mesh.m_indexType;
+}
+
+void btTriangleIndexVertexArray::getLockedReadOnlyVertexIndexBase(const unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& vertexStride,const unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart) const
+{
+ const btIndexedMesh& mesh = m_indexedMeshes[subpart];
+
+ numverts = mesh.m_numVertices;
+ (*vertexbase) = (const unsigned char *)mesh.m_vertexBase;
+
+ type = mesh.m_vertexType;
+
+ vertexStride = mesh.m_vertexStride;
+
+ numfaces = mesh.m_numTriangles;
+ (*indexbase) = (const unsigned char *)mesh.m_triangleIndexBase;
+ indexstride = mesh.m_triangleIndexStride;
+ indicestype = mesh.m_indexType;
+}
+
+bool btTriangleIndexVertexArray::hasPremadeAabb() const
+{
+ return (m_hasAabb == 1);
+}
+
+
+void btTriangleIndexVertexArray::setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax ) const
+{
+ m_aabbMin = aabbMin;
+ m_aabbMax = aabbMax;
+ m_hasAabb = 1; // this is intentionally an int see notes in header
+}
+
+void btTriangleIndexVertexArray::getPremadeAabb(btVector3* aabbMin, btVector3* aabbMax ) const
+{
+ *aabbMin = m_aabbMin;
+ *aabbMax = m_aabbMax;
+}
+
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btTriangleIndexVertexMaterialArray.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btTriangleIndexVertexMaterialArray.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btTriangleIndexVertexMaterialArray.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btTriangleIndexVertexMaterialArray.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,86 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///This file was created by Alex Silverman
+
+#include "BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h"
+
+btTriangleIndexVertexMaterialArray::btTriangleIndexVertexMaterialArray(int numTriangles,int* triangleIndexBase,int triangleIndexStride,
+ int numVertices,btScalar* vertexBase,int vertexStride,
+ int numMaterials, unsigned char* materialBase, int materialStride,
+ int* triangleMaterialsBase, int materialIndexStride) :
+btTriangleIndexVertexArray(numTriangles, triangleIndexBase, triangleIndexStride, numVertices, vertexBase, vertexStride)
+{
+ btMaterialProperties mat;
+
+ mat.m_numMaterials = numMaterials;
+ mat.m_materialBase = materialBase;
+ mat.m_materialStride = materialStride;
+#ifdef BT_USE_DOUBLE_PRECISION
+ mat.m_materialType = PHY_DOUBLE;
+#else
+ mat.m_materialType = PHY_FLOAT;
+#endif
+
+ mat.m_numTriangles = numTriangles;
+ mat.m_triangleMaterialsBase = (unsigned char *)triangleMaterialsBase;
+ mat.m_triangleMaterialStride = materialIndexStride;
+ mat.m_triangleType = PHY_INTEGER;
+
+ addMaterialProperties(mat);
+}
+
+
+void btTriangleIndexVertexMaterialArray::getLockedMaterialBase(unsigned char **materialBase, int& numMaterials, PHY_ScalarType& materialType, int& materialStride,
+ unsigned char ** triangleMaterialBase, int& numTriangles, int& triangleMaterialStride, PHY_ScalarType& triangleType, int subpart)
+{
+ btAssert(subpart< getNumSubParts() );
+
+ btMaterialProperties& mats = m_materials[subpart];
+
+ numMaterials = mats.m_numMaterials;
+ (*materialBase) = (unsigned char *) mats.m_materialBase;
+#ifdef BT_USE_DOUBLE_PRECISION
+ materialType = PHY_DOUBLE;
+#else
+ materialType = PHY_FLOAT;
+#endif
+ materialStride = mats.m_materialStride;
+
+ numTriangles = mats.m_numTriangles;
+ (*triangleMaterialBase) = (unsigned char *)mats.m_triangleMaterialsBase;
+ triangleMaterialStride = mats.m_triangleMaterialStride;
+ triangleType = mats.m_triangleType;
+}
+
+void btTriangleIndexVertexMaterialArray::getLockedReadOnlyMaterialBase(const unsigned char **materialBase, int& numMaterials, PHY_ScalarType& materialType, int& materialStride,
+ const unsigned char ** triangleMaterialBase, int& numTriangles, int& triangleMaterialStride, PHY_ScalarType& triangleType, int subpart)
+{
+ btMaterialProperties& mats = m_materials[subpart];
+
+ numMaterials = mats.m_numMaterials;
+ (*materialBase) = (const unsigned char *) mats.m_materialBase;
+#ifdef BT_USE_DOUBLE_PRECISION
+ materialType = PHY_DOUBLE;
+#else
+ materialType = PHY_FLOAT;
+#endif
+ materialStride = mats.m_materialStride;
+
+ numTriangles = mats.m_numTriangles;
+ (*triangleMaterialBase) = (const unsigned char *)mats.m_triangleMaterialsBase;
+ triangleMaterialStride = mats.m_triangleMaterialStride;
+ triangleType = mats.m_triangleType;
+}
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btTriangleMesh.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btTriangleMesh.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btTriangleMesh.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btTriangleMesh.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,140 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "BulletCollision/CollisionShapes/btTriangleMesh.h"
+
+
+
+btTriangleMesh::btTriangleMesh (bool use32bitIndices,bool use4componentVertices)
+:m_use32bitIndices(use32bitIndices),
+m_use4componentVertices(use4componentVertices),
+m_weldingThreshold(0.0)
+{
+ btIndexedMesh meshIndex;
+ meshIndex.m_numTriangles = 0;
+ meshIndex.m_numVertices = 0;
+ meshIndex.m_indexType = PHY_INTEGER;
+ meshIndex.m_triangleIndexBase = 0;
+ meshIndex.m_triangleIndexStride = 3*sizeof(int);
+ meshIndex.m_vertexBase = 0;
+ meshIndex.m_vertexStride = sizeof(btVector3);
+ m_indexedMeshes.push_back(meshIndex);
+
+ if (m_use32bitIndices)
+ {
+ m_indexedMeshes[0].m_numTriangles = m_32bitIndices.size()/3;
+ m_indexedMeshes[0].m_triangleIndexBase = 0;
+ m_indexedMeshes[0].m_indexType = PHY_INTEGER;
+ m_indexedMeshes[0].m_triangleIndexStride = 3*sizeof(int);
+ } else
+ {
+ m_indexedMeshes[0].m_numTriangles = m_16bitIndices.size()/3;
+ m_indexedMeshes[0].m_triangleIndexBase = 0;
+ m_indexedMeshes[0].m_indexType = PHY_SHORT;
+ m_indexedMeshes[0].m_triangleIndexStride = 3*sizeof(short int);
+ }
+
+ if (m_use4componentVertices)
+ {
+ m_indexedMeshes[0].m_numVertices = m_4componentVertices.size();
+ m_indexedMeshes[0].m_vertexBase = 0;
+ m_indexedMeshes[0].m_vertexStride = sizeof(btVector3);
+ } else
+ {
+ m_indexedMeshes[0].m_numVertices = m_3componentVertices.size()/3;
+ m_indexedMeshes[0].m_vertexBase = 0;
+ m_indexedMeshes[0].m_vertexStride = 3*sizeof(btScalar);
+ }
+
+
+}
+
+void btTriangleMesh::addIndex(int index)
+{
+ if (m_use32bitIndices)
+ {
+ m_32bitIndices.push_back(index);
+ m_indexedMeshes[0].m_triangleIndexBase = (unsigned char*) &m_32bitIndices[0];
+ } else
+ {
+ m_16bitIndices.push_back(index);
+ m_indexedMeshes[0].m_triangleIndexBase = (unsigned char*) &m_16bitIndices[0];
+ }
+}
+
+
+int btTriangleMesh::findOrAddVertex(const btVector3& vertex, bool removeDuplicateVertices)
+{
+ //return index of new/existing vertex
+ ///@todo: could use acceleration structure for this
+ if (m_use4componentVertices)
+ {
+ if (removeDuplicateVertices)
+ {
+ for (int i=0;i< m_4componentVertices.size();i++)
+ {
+ if ((m_4componentVertices[i]-vertex).length2() <= m_weldingThreshold)
+ {
+ return i;
+ }
+ }
+ }
+ m_indexedMeshes[0].m_numVertices++;
+ m_4componentVertices.push_back(vertex);
+ m_indexedMeshes[0].m_vertexBase = (unsigned char*)&m_4componentVertices[0];
+
+ return m_4componentVertices.size()-1;
+
+ } else
+ {
+
+ if (removeDuplicateVertices)
+ {
+ for (int i=0;i< m_3componentVertices.size();i+=3)
+ {
+ btVector3 vtx(m_3componentVertices[i],m_3componentVertices[i+1],m_3componentVertices[i+2]);
+ if ((vtx-vertex).length2() <= m_weldingThreshold)
+ {
+ return i/3;
+ }
+ }
+ }
+ m_3componentVertices.push_back((float)vertex.getX());
+ m_3componentVertices.push_back((float)vertex.getY());
+ m_3componentVertices.push_back((float)vertex.getZ());
+ m_indexedMeshes[0].m_numVertices++;
+ m_indexedMeshes[0].m_vertexBase = (unsigned char*)&m_3componentVertices[0];
+ return (m_3componentVertices.size()/3)-1;
+ }
+
+}
+
+void btTriangleMesh::addTriangle(const btVector3& vertex0,const btVector3& vertex1,const btVector3& vertex2,bool removeDuplicateVertices)
+{
+ m_indexedMeshes[0].m_numTriangles++;
+ addIndex(findOrAddVertex(vertex0,removeDuplicateVertices));
+ addIndex(findOrAddVertex(vertex1,removeDuplicateVertices));
+ addIndex(findOrAddVertex(vertex2,removeDuplicateVertices));
+}
+
+int btTriangleMesh::getNumTriangles() const
+{
+ if (m_use32bitIndices)
+ {
+ return m_32bitIndices.size() / 3;
+ }
+ return m_16bitIndices.size() / 3;
+}
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btTriangleMeshShape.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btTriangleMeshShape.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btTriangleMeshShape.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btTriangleMeshShape.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,209 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "BulletCollision/CollisionShapes/btTriangleMeshShape.h"
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btQuaternion.h"
+#include "BulletCollision/CollisionShapes/btStridingMeshInterface.h"
+#include "LinearMath/btAabbUtil2.h"
+#include "BulletCollision/CollisionShapes/btCollisionMargin.h"
+
+
+btTriangleMeshShape::btTriangleMeshShape(btStridingMeshInterface* meshInterface)
+: btConcaveShape (), m_meshInterface(meshInterface)
+{
+ m_shapeType = TRIANGLE_MESH_SHAPE_PROXYTYPE;
+ if(meshInterface->hasPremadeAabb())
+ {
+ meshInterface->getPremadeAabb(&m_localAabbMin, &m_localAabbMax);
+ }
+ else
+ {
+ recalcLocalAabb();
+ }
+}
+
+
+btTriangleMeshShape::~btTriangleMeshShape()
+{
+
+}
+
+
+
+
+void btTriangleMeshShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const
+{
+
+ btVector3 localHalfExtents = btScalar(0.5)*(m_localAabbMax-m_localAabbMin);
+ localHalfExtents += btVector3(getMargin(),getMargin(),getMargin());
+ btVector3 localCenter = btScalar(0.5)*(m_localAabbMax+m_localAabbMin);
+
+ btMatrix3x3 abs_b = trans.getBasis().absolute();
+
+ btVector3 center = trans(localCenter);
+
+ btVector3 extent = btVector3(abs_b[0].dot(localHalfExtents),
+ abs_b[1].dot(localHalfExtents),
+ abs_b[2].dot(localHalfExtents));
+ aabbMin = center - extent;
+ aabbMax = center + extent;
+
+
+}
+
+void btTriangleMeshShape::recalcLocalAabb()
+{
+ for (int i=0;i<3;i++)
+ {
+ btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.));
+ vec[i] = btScalar(1.);
+ btVector3 tmp = localGetSupportingVertex(vec);
+ m_localAabbMax[i] = tmp[i]+m_collisionMargin;
+ vec[i] = btScalar(-1.);
+ tmp = localGetSupportingVertex(vec);
+ m_localAabbMin[i] = tmp[i]-m_collisionMargin;
+ }
+}
+
+
+
+class SupportVertexCallback : public btTriangleCallback
+{
+
+ btVector3 m_supportVertexLocal;
+public:
+
+ btTransform m_worldTrans;
+ btScalar m_maxDot;
+ btVector3 m_supportVecLocal;
+
+ SupportVertexCallback(const btVector3& supportVecWorld,const btTransform& trans)
+ : m_supportVertexLocal(btScalar(0.),btScalar(0.),btScalar(0.)), m_worldTrans(trans) ,m_maxDot(btScalar(-BT_LARGE_FLOAT))
+
+ {
+ m_supportVecLocal = supportVecWorld * m_worldTrans.getBasis();
+ }
+
+ virtual void processTriangle( btVector3* triangle,int partId, int triangleIndex)
+ {
+ (void)partId;
+ (void)triangleIndex;
+ for (int i=0;i<3;i++)
+ {
+ btScalar dot = m_supportVecLocal.dot(triangle[i]);
+ if (dot > m_maxDot)
+ {
+ m_maxDot = dot;
+ m_supportVertexLocal = triangle[i];
+ }
+ }
+ }
+
+ btVector3 GetSupportVertexWorldSpace()
+ {
+ return m_worldTrans(m_supportVertexLocal);
+ }
+
+ btVector3 GetSupportVertexLocal()
+ {
+ return m_supportVertexLocal;
+ }
+
+};
+
+
+void btTriangleMeshShape::setLocalScaling(const btVector3& scaling)
+{
+ m_meshInterface->setScaling(scaling);
+ recalcLocalAabb();
+}
+
+const btVector3& btTriangleMeshShape::getLocalScaling() const
+{
+ return m_meshInterface->getScaling();
+}
+
+
+
+
+
+
+//#define DEBUG_TRIANGLE_MESH
+
+
+
+void btTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
+{
+ struct FilteredCallback : public btInternalTriangleIndexCallback
+ {
+ btTriangleCallback* m_callback;
+ btVector3 m_aabbMin;
+ btVector3 m_aabbMax;
+
+ FilteredCallback(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax)
+ :m_callback(callback),
+ m_aabbMin(aabbMin),
+ m_aabbMax(aabbMax)
+ {
+ }
+
+ virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex)
+ {
+ if (TestTriangleAgainstAabb2(&triangle[0],m_aabbMin,m_aabbMax))
+ {
+ //check aabb in triangle-space, before doing this
+ m_callback->processTriangle(triangle,partId,triangleIndex);
+ }
+
+ }
+
+ };
+
+ FilteredCallback filterCallback(callback,aabbMin,aabbMax);
+
+ m_meshInterface->InternalProcessAllTriangles(&filterCallback,aabbMin,aabbMax);
+}
+
+
+
+
+
+void btTriangleMeshShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+{
+ (void)mass;
+ //moving concave objects not supported
+ btAssert(0);
+ inertia.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+}
+
+
+btVector3 btTriangleMeshShape::localGetSupportingVertex(const btVector3& vec) const
+{
+ btVector3 supportVertex;
+
+ btTransform ident;
+ ident.setIdentity();
+
+ SupportVertexCallback supportCallback(vec,ident);
+
+ btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
+
+ processAllTriangles(&supportCallback,-aabbMax,aabbMax);
+
+ supportVertex = supportCallback.GetSupportVertexLocal();
+
+ return supportVertex;
+}
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btTriangleShapeEx.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btTriangleShapeEx.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btTriangleShapeEx.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btTriangleShapeEx.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,218 @@
+/*! \file btGImpactTriangleShape.h
+\author Francisco Len Nßjera
+*/
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman at yahoo.com
+
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "BulletCollision/Gimpact/btTriangleShapeEx.h"
+
+
+
+void GIM_TRIANGLE_CONTACT::merge_points(const btVector4 & plane,
+ btScalar margin, const btVector3 * points, int point_count)
+{
+ m_point_count = 0;
+ m_penetration_depth= -1000.0f;
+
+ int point_indices[MAX_TRI_CLIPPING];
+
+ int _k;
+
+ for ( _k=0;_k<point_count;_k++)
+ {
+ btScalar _dist = - bt_distance_point_plane(plane,points[_k]) + margin;
+
+ if (_dist>=0.0f)
+ {
+ if (_dist>m_penetration_depth)
+ {
+ m_penetration_depth = _dist;
+ point_indices[0] = _k;
+ m_point_count=1;
+ }
+ else if ((_dist+SIMD_EPSILON)>=m_penetration_depth)
+ {
+ point_indices[m_point_count] = _k;
+ m_point_count++;
+ }
+ }
+ }
+
+ for ( _k=0;_k<m_point_count;_k++)
+ {
+ m_points[_k] = points[point_indices[_k]];
+ }
+}
+
+///class btPrimitiveTriangle
+bool btPrimitiveTriangle::overlap_test_conservative(const btPrimitiveTriangle& other)
+{
+ btScalar total_margin = m_margin + other.m_margin;
+ // classify points on other triangle
+ btScalar dis0 = bt_distance_point_plane(m_plane,other.m_vertices[0]) - total_margin;
+
+ btScalar dis1 = bt_distance_point_plane(m_plane,other.m_vertices[1]) - total_margin;
+
+ btScalar dis2 = bt_distance_point_plane(m_plane,other.m_vertices[2]) - total_margin;
+
+ if (dis0>0.0f&&dis1>0.0f&&dis2>0.0f) return false;
+
+ // classify points on this triangle
+ dis0 = bt_distance_point_plane(other.m_plane,m_vertices[0]) - total_margin;
+
+ dis1 = bt_distance_point_plane(other.m_plane,m_vertices[1]) - total_margin;
+
+ dis2 = bt_distance_point_plane(other.m_plane,m_vertices[2]) - total_margin;
+
+ if (dis0>0.0f&&dis1>0.0f&&dis2>0.0f) return false;
+
+ return true;
+}
+
+int btPrimitiveTriangle::clip_triangle(btPrimitiveTriangle & other, btVector3 * clipped_points )
+{
+ // edge 0
+
+ btVector3 temp_points[MAX_TRI_CLIPPING];
+
+
+ btVector4 edgeplane;
+
+ get_edge_plane(0,edgeplane);
+
+
+ int clipped_count = bt_plane_clip_triangle(
+ edgeplane,other.m_vertices[0],other.m_vertices[1],other.m_vertices[2],temp_points);
+
+ if (clipped_count == 0) return 0;
+
+ btVector3 temp_points1[MAX_TRI_CLIPPING];
+
+
+ // edge 1
+ get_edge_plane(1,edgeplane);
+
+
+ clipped_count = bt_plane_clip_polygon(edgeplane,temp_points,clipped_count,temp_points1);
+
+ if (clipped_count == 0) return 0;
+
+ // edge 2
+ get_edge_plane(2,edgeplane);
+
+ clipped_count = bt_plane_clip_polygon(
+ edgeplane,temp_points1,clipped_count,clipped_points);
+
+ return clipped_count;
+}
+
+bool btPrimitiveTriangle::find_triangle_collision_clip_method(btPrimitiveTriangle & other, GIM_TRIANGLE_CONTACT & contacts)
+{
+ btScalar margin = m_margin + other.m_margin;
+
+ btVector3 clipped_points[MAX_TRI_CLIPPING];
+ int clipped_count;
+ //create planes
+ // plane v vs U points
+
+ GIM_TRIANGLE_CONTACT contacts1;
+
+ contacts1.m_separating_normal = m_plane;
+
+
+ clipped_count = clip_triangle(other,clipped_points);
+
+ if (clipped_count == 0 )
+ {
+ return false;//Reject
+ }
+
+ //find most deep interval face1
+ contacts1.merge_points(contacts1.m_separating_normal,margin,clipped_points,clipped_count);
+ if (contacts1.m_point_count == 0) return false; // too far
+ //Normal pointing to this triangle
+ contacts1.m_separating_normal *= -1.f;
+
+
+ //Clip tri1 by tri2 edges
+ GIM_TRIANGLE_CONTACT contacts2;
+ contacts2.m_separating_normal = other.m_plane;
+
+ clipped_count = other.clip_triangle(*this,clipped_points);
+
+ if (clipped_count == 0 )
+ {
+ return false;//Reject
+ }
+
+ //find most deep interval face1
+ contacts2.merge_points(contacts2.m_separating_normal,margin,clipped_points,clipped_count);
+ if (contacts2.m_point_count == 0) return false; // too far
+
+
+
+
+ ////check most dir for contacts
+ if (contacts2.m_penetration_depth<contacts1.m_penetration_depth)
+ {
+ contacts.copy_from(contacts2);
+ }
+ else
+ {
+ contacts.copy_from(contacts1);
+ }
+ return true;
+}
+
+
+
+///class btTriangleShapeEx: public btTriangleShape
+
+bool btTriangleShapeEx::overlap_test_conservative(const btTriangleShapeEx& other)
+{
+ btScalar total_margin = getMargin() + other.getMargin();
+
+ btVector4 plane0;
+ buildTriPlane(plane0);
+ btVector4 plane1;
+ other.buildTriPlane(plane1);
+
+ // classify points on other triangle
+ btScalar dis0 = bt_distance_point_plane(plane0,other.m_vertices1[0]) - total_margin;
+
+ btScalar dis1 = bt_distance_point_plane(plane0,other.m_vertices1[1]) - total_margin;
+
+ btScalar dis2 = bt_distance_point_plane(plane0,other.m_vertices1[2]) - total_margin;
+
+ if (dis0>0.0f&&dis1>0.0f&&dis2>0.0f) return false;
+
+ // classify points on this triangle
+ dis0 = bt_distance_point_plane(plane1,m_vertices1[0]) - total_margin;
+
+ dis1 = bt_distance_point_plane(plane1,m_vertices1[1]) - total_margin;
+
+ dis2 = bt_distance_point_plane(plane1,m_vertices1[2]) - total_margin;
+
+ if (dis0>0.0f&&dis1>0.0f&&dis2>0.0f) return false;
+
+ return true;
+}
+
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btTypedConstraint.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btTypedConstraint.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btTypedConstraint.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btTypedConstraint.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,116 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+
+static btRigidBody s_fixed(0, 0,0);
+
+#define DEFAULT_DEBUGDRAW_SIZE btScalar(0.3f)
+
+btTypedConstraint::btTypedConstraint(btTypedConstraintType type)
+:btTypedObject(type),
+m_userConstraintType(-1),
+m_userConstraintId(-1),
+m_needsFeedback(false),
+m_rbA(s_fixed),
+m_rbB(s_fixed),
+m_appliedImpulse(btScalar(0.)),
+m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE)
+{
+ s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
+}
+btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA)
+:btTypedObject(type),
+m_userConstraintType(-1),
+m_userConstraintId(-1),
+m_needsFeedback(false),
+m_rbA(rbA),
+m_rbB(s_fixed),
+m_appliedImpulse(btScalar(0.)),
+m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE)
+{
+ s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
+}
+
+
+btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB)
+:btTypedObject(type),
+m_userConstraintType(-1),
+m_userConstraintId(-1),
+m_needsFeedback(false),
+m_rbA(rbA),
+m_rbB(rbB),
+m_appliedImpulse(btScalar(0.)),
+m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE)
+{
+ s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
+
+}
+
+
+
+
+btScalar btTypedConstraint::getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact)
+{
+ if(lowLim > uppLim)
+ {
+ return btScalar(1.0f);
+ }
+ else if(lowLim == uppLim)
+ {
+ return btScalar(0.0f);
+ }
+ btScalar lim_fact = btScalar(1.0f);
+ btScalar delta_max = vel / timeFact;
+ if(delta_max < btScalar(0.0f))
+ {
+ if((pos >= lowLim) && (pos < (lowLim - delta_max)))
+ {
+ lim_fact = (lowLim - pos) / delta_max;
+ }
+ else if(pos < lowLim)
+ {
+ lim_fact = btScalar(0.0f);
+ }
+ else
+ {
+ lim_fact = btScalar(1.0f);
+ }
+ }
+ else if(delta_max > btScalar(0.0f))
+ {
+ if((pos <= uppLim) && (pos > (uppLim - delta_max)))
+ {
+ lim_fact = (uppLim - pos) / delta_max;
+ }
+ else if(pos > uppLim)
+ {
+ lim_fact = btScalar(0.0f);
+ }
+ else
+ {
+ lim_fact = btScalar(1.0f);
+ }
+ }
+ else
+ {
+ lim_fact = btScalar(0.0f);
+ }
+ return lim_fact;
+}
+
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btUniformScalingShape.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btUniformScalingShape.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btUniformScalingShape.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btUniformScalingShape.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,115 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "BulletCollision/CollisionShapes/btUniformScalingShape.h"
+
+btUniformScalingShape::btUniformScalingShape( btConvexShape* convexChildShape,btScalar uniformScalingFactor):
+btConvexShape (), m_childConvexShape(convexChildShape),
+m_uniformScalingFactor(uniformScalingFactor)
+{
+ m_shapeType = UNIFORM_SCALING_SHAPE_PROXYTYPE;
+}
+
+btUniformScalingShape::~btUniformScalingShape()
+{
+}
+
+
+btVector3 btUniformScalingShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const
+{
+ btVector3 tmpVertex;
+ tmpVertex = m_childConvexShape->localGetSupportingVertexWithoutMargin(vec);
+ return tmpVertex*m_uniformScalingFactor;
+}
+
+void btUniformScalingShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
+{
+ m_childConvexShape->batchedUnitVectorGetSupportingVertexWithoutMargin(vectors,supportVerticesOut,numVectors);
+ int i;
+ for (i=0;i<numVectors;i++)
+ {
+ supportVerticesOut[i] = supportVerticesOut[i] * m_uniformScalingFactor;
+ }
+}
+
+
+btVector3 btUniformScalingShape::localGetSupportingVertex(const btVector3& vec)const
+{
+ btVector3 tmpVertex;
+ tmpVertex = m_childConvexShape->localGetSupportingVertex(vec);
+ return tmpVertex*m_uniformScalingFactor;
+}
+
+
+void btUniformScalingShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+{
+
+ ///this linear upscaling is not realistic, but we don't deal with large mass ratios...
+ btVector3 tmpInertia;
+ m_childConvexShape->calculateLocalInertia(mass,tmpInertia);
+ inertia = tmpInertia * m_uniformScalingFactor;
+}
+
+
+ ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
+void btUniformScalingShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
+{
+ m_childConvexShape->getAabb(t,aabbMin,aabbMax);
+ btVector3 aabbCenter = (aabbMax+aabbMin)*btScalar(0.5);
+ btVector3 scaledAabbHalfExtends = (aabbMax-aabbMin)*btScalar(0.5)*m_uniformScalingFactor;
+
+ aabbMin = aabbCenter - scaledAabbHalfExtends;
+ aabbMax = aabbCenter + scaledAabbHalfExtends;
+
+}
+
+void btUniformScalingShape::getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
+{
+ m_childConvexShape->getAabbSlow(t,aabbMin,aabbMax);
+ btVector3 aabbCenter = (aabbMax+aabbMin)*btScalar(0.5);
+ btVector3 scaledAabbHalfExtends = (aabbMax-aabbMin)*btScalar(0.5)*m_uniformScalingFactor;
+
+ aabbMin = aabbCenter - scaledAabbHalfExtends;
+ aabbMax = aabbCenter + scaledAabbHalfExtends;
+}
+
+void btUniformScalingShape::setLocalScaling(const btVector3& scaling)
+{
+ m_childConvexShape->setLocalScaling(scaling);
+}
+
+const btVector3& btUniformScalingShape::getLocalScaling() const
+{
+ return m_childConvexShape->getLocalScaling();
+}
+
+void btUniformScalingShape::setMargin(btScalar margin)
+{
+ m_childConvexShape->setMargin(margin);
+}
+btScalar btUniformScalingShape::getMargin() const
+{
+ return m_childConvexShape->getMargin() * m_uniformScalingFactor;
+}
+
+int btUniformScalingShape::getNumPreferredPenetrationDirections() const
+{
+ return m_childConvexShape->getNumPreferredPenetrationDirections();
+}
+
+void btUniformScalingShape::getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const
+{
+ m_childConvexShape->getPreferredPenetrationDirection(index,penetrationVector);
+}
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btUnionFind.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btUnionFind.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btUnionFind.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btUnionFind.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,81 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "BulletCollision/CollisionDispatch/btUnionFind.h"
+
+
+
+btUnionFind::~btUnionFind()
+{
+ Free();
+
+}
+
+btUnionFind::btUnionFind()
+{
+
+}
+
+void btUnionFind::allocate(int N)
+{
+ m_elements.resize(N);
+}
+void btUnionFind::Free()
+{
+ m_elements.clear();
+}
+
+
+void btUnionFind::reset(int N)
+{
+ allocate(N);
+
+ for (int i = 0; i < N; i++)
+ {
+ m_elements[i].m_id = i; m_elements[i].m_sz = 1;
+ }
+}
+
+
+class btUnionFindElementSortPredicate
+{
+ public:
+
+ bool operator() ( const btElement& lhs, const btElement& rhs )
+ {
+ return lhs.m_id < rhs.m_id;
+ }
+};
+
+///this is a special operation, destroying the content of btUnionFind.
+///it sorts the elements, based on island id, in order to make it easy to iterate over islands
+void btUnionFind::sortIslands()
+{
+
+ //first store the original body index, and islandId
+ int numElements = m_elements.size();
+
+ for (int i=0;i<numElements;i++)
+ {
+ m_elements[i].m_id = find(i);
+ m_elements[i].m_sz = i;
+ }
+
+ // Sort the vector using predicate and std::sort
+ //std::sort(m_elements.begin(), m_elements.end(), btUnionFindElementSortPredicate);
+ m_elements.quickSort(btUnionFindElementSortPredicate());
+
+}
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btUniversalConstraint.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btUniversalConstraint.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btUniversalConstraint.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btUniversalConstraint.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,63 @@
+/*
+Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org
+Copyright (C) 2006, 2007 Sony Computer Entertainment Inc.
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#include "BulletDynamics/ConstraintSolver/btUniversalConstraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btTransformUtil.h"
+
+
+
+#define UNIV_EPS btScalar(0.01f)
+
+
+// constructor
+// anchor, axis1 and axis2 are in world coordinate system
+// axis1 must be orthogonal to axis2
+btUniversalConstraint::btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2)
+: btGeneric6DofConstraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(), true),
+ m_anchor(anchor),
+ m_axis1(axis1),
+ m_axis2(axis2)
+{
+ // build frame basis
+ // 6DOF constraint uses Euler angles and to define limits
+ // it is assumed that rotational order is :
+ // Z - first, allowed limits are (-PI,PI);
+ // new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number
+ // used to prevent constraint from instability on poles;
+ // new position of X, allowed limits are (-PI,PI);
+ // So to simulate ODE Universal joint we should use parent axis as Z, child axis as Y and limit all other DOFs
+ // Build the frame in world coordinate system first
+ btVector3 zAxis = axis1.normalize();
+ btVector3 yAxis = axis2.normalize();
+ btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
+ btTransform frameInW;
+ frameInW.setIdentity();
+ frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0],
+ xAxis[1], yAxis[1], zAxis[1],
+ xAxis[2], yAxis[2], zAxis[2]);
+ frameInW.setOrigin(anchor);
+ // now get constraint frame in local coordinate systems
+ m_frameInA = rbA.getCenterOfMassTransform().inverse() * frameInW;
+ m_frameInB = rbB.getCenterOfMassTransform().inverse() * frameInW;
+ // sei limits
+ setLinearLowerLimit(btVector3(0., 0., 0.));
+ setLinearUpperLimit(btVector3(0., 0., 0.));
+ setAngularLowerLimit(btVector3(0.f, -SIMD_HALF_PI + UNIV_EPS, -SIMD_PI + UNIV_EPS));
+ setAngularUpperLimit(btVector3(0.f, SIMD_HALF_PI - UNIV_EPS, SIMD_PI - UNIV_EPS));
+}
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btVoronoiSimplexSolver.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btVoronoiSimplexSolver.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btVoronoiSimplexSolver.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btVoronoiSimplexSolver.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,605 @@
+
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+
+ Elsevier CDROM license agreements grants nonexclusive license to use the software
+ for any purpose, commercial or non-commercial as long as the following credit is included
+ identifying the original source of the software:
+
+ Parts of the source are "from the book Real-Time Collision Detection by
+ Christer Ericson, published by Morgan Kaufmann Publishers,
+ (c) 2005 Elsevier Inc."
+
+*/
+
+
+#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
+
+#define VERTA 0
+#define VERTB 1
+#define VERTC 2
+#define VERTD 3
+
+#define CATCH_DEGENERATE_TETRAHEDRON 1
+void btVoronoiSimplexSolver::removeVertex(int index)
+{
+
+ btAssert(m_numVertices>0);
+ m_numVertices--;
+ m_simplexVectorW[index] = m_simplexVectorW[m_numVertices];
+ m_simplexPointsP[index] = m_simplexPointsP[m_numVertices];
+ m_simplexPointsQ[index] = m_simplexPointsQ[m_numVertices];
+}
+
+void btVoronoiSimplexSolver::reduceVertices (const btUsageBitfield& usedVerts)
+{
+ if ((numVertices() >= 4) && (!usedVerts.usedVertexD))
+ removeVertex(3);
+
+ if ((numVertices() >= 3) && (!usedVerts.usedVertexC))
+ removeVertex(2);
+
+ if ((numVertices() >= 2) && (!usedVerts.usedVertexB))
+ removeVertex(1);
+
+ if ((numVertices() >= 1) && (!usedVerts.usedVertexA))
+ removeVertex(0);
+
+}
+
+
+
+
+
+//clear the simplex, remove all the vertices
+void btVoronoiSimplexSolver::reset()
+{
+ m_cachedValidClosest = false;
+ m_numVertices = 0;
+ m_needsUpdate = true;
+ m_lastW = btVector3(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
+ m_cachedBC.reset();
+}
+
+
+
+ //add a vertex
+void btVoronoiSimplexSolver::addVertex(const btVector3& w, const btVector3& p, const btVector3& q)
+{
+ m_lastW = w;
+ m_needsUpdate = true;
+
+ m_simplexVectorW[m_numVertices] = w;
+ m_simplexPointsP[m_numVertices] = p;
+ m_simplexPointsQ[m_numVertices] = q;
+
+ m_numVertices++;
+}
+
+bool btVoronoiSimplexSolver::updateClosestVectorAndPoints()
+{
+
+ if (m_needsUpdate)
+ {
+ m_cachedBC.reset();
+
+ m_needsUpdate = false;
+
+ switch (numVertices())
+ {
+ case 0:
+ m_cachedValidClosest = false;
+ break;
+ case 1:
+ {
+ m_cachedP1 = m_simplexPointsP[0];
+ m_cachedP2 = m_simplexPointsQ[0];
+ m_cachedV = m_cachedP1-m_cachedP2; //== m_simplexVectorW[0]
+ m_cachedBC.reset();
+ m_cachedBC.setBarycentricCoordinates(btScalar(1.),btScalar(0.),btScalar(0.),btScalar(0.));
+ m_cachedValidClosest = m_cachedBC.isValid();
+ break;
+ };
+ case 2:
+ {
+ //closest point origin from line segment
+ const btVector3& from = m_simplexVectorW[0];
+ const btVector3& to = m_simplexVectorW[1];
+ btVector3 nearest;
+
+ btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.));
+ btVector3 diff = p - from;
+ btVector3 v = to - from;
+ btScalar t = v.dot(diff);
+
+ if (t > 0) {
+ btScalar dotVV = v.dot(v);
+ if (t < dotVV) {
+ t /= dotVV;
+ diff -= t*v;
+ m_cachedBC.m_usedVertices.usedVertexA = true;
+ m_cachedBC.m_usedVertices.usedVertexB = true;
+ } else {
+ t = 1;
+ diff -= v;
+ //reduce to 1 point
+ m_cachedBC.m_usedVertices.usedVertexB = true;
+ }
+ } else
+ {
+ t = 0;
+ //reduce to 1 point
+ m_cachedBC.m_usedVertices.usedVertexA = true;
+ }
+ m_cachedBC.setBarycentricCoordinates(1-t,t);
+ nearest = from + t*v;
+
+ m_cachedP1 = m_simplexPointsP[0] + t * (m_simplexPointsP[1] - m_simplexPointsP[0]);
+ m_cachedP2 = m_simplexPointsQ[0] + t * (m_simplexPointsQ[1] - m_simplexPointsQ[0]);
+ m_cachedV = m_cachedP1 - m_cachedP2;
+
+ reduceVertices(m_cachedBC.m_usedVertices);
+
+ m_cachedValidClosest = m_cachedBC.isValid();
+ break;
+ }
+ case 3:
+ {
+ //closest point origin from triangle
+ btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.));
+
+ const btVector3& a = m_simplexVectorW[0];
+ const btVector3& b = m_simplexVectorW[1];
+ const btVector3& c = m_simplexVectorW[2];
+
+ closestPtPointTriangle(p,a,b,c,m_cachedBC);
+ m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] +
+ m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] +
+ m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2];
+
+ m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] +
+ m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] +
+ m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2];
+
+ m_cachedV = m_cachedP1-m_cachedP2;
+
+ reduceVertices (m_cachedBC.m_usedVertices);
+ m_cachedValidClosest = m_cachedBC.isValid();
+
+ break;
+ }
+ case 4:
+ {
+
+
+ btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.));
+
+ const btVector3& a = m_simplexVectorW[0];
+ const btVector3& b = m_simplexVectorW[1];
+ const btVector3& c = m_simplexVectorW[2];
+ const btVector3& d = m_simplexVectorW[3];
+
+ bool hasSeperation = closestPtPointTetrahedron(p,a,b,c,d,m_cachedBC);
+
+ if (hasSeperation)
+ {
+
+ m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] +
+ m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] +
+ m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] +
+ m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3];
+
+ m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] +
+ m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] +
+ m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] +
+ m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3];
+
+ m_cachedV = m_cachedP1-m_cachedP2;
+ reduceVertices (m_cachedBC.m_usedVertices);
+ } else
+ {
+// printf("sub distance got penetration\n");
+
+ if (m_cachedBC.m_degenerate)
+ {
+ m_cachedValidClosest = false;
+ } else
+ {
+ m_cachedValidClosest = true;
+ //degenerate case == false, penetration = true + zero
+ m_cachedV.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+ }
+ break;
+ }
+
+ m_cachedValidClosest = m_cachedBC.isValid();
+
+ //closest point origin from tetrahedron
+ break;
+ }
+ default:
+ {
+ m_cachedValidClosest = false;
+ }
+ };
+ }
+
+ return m_cachedValidClosest;
+
+}
+
+//return/calculate the closest vertex
+bool btVoronoiSimplexSolver::closest(btVector3& v)
+{
+ bool succes = updateClosestVectorAndPoints();
+ v = m_cachedV;
+ return succes;
+}
+
+
+
+btScalar btVoronoiSimplexSolver::maxVertex()
+{
+ int i, numverts = numVertices();
+ btScalar maxV = btScalar(0.);
+ for (i=0;i<numverts;i++)
+ {
+ btScalar curLen2 = m_simplexVectorW[i].length2();
+ if (maxV < curLen2)
+ maxV = curLen2;
+ }
+ return maxV;
+}
+
+
+
+ //return the current simplex
+int btVoronoiSimplexSolver::getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const
+{
+ int i;
+ for (i=0;i<numVertices();i++)
+ {
+ yBuf[i] = m_simplexVectorW[i];
+ pBuf[i] = m_simplexPointsP[i];
+ qBuf[i] = m_simplexPointsQ[i];
+ }
+ return numVertices();
+}
+
+
+
+
+bool btVoronoiSimplexSolver::inSimplex(const btVector3& w)
+{
+ bool found = false;
+ int i, numverts = numVertices();
+ //btScalar maxV = btScalar(0.);
+
+ //w is in the current (reduced) simplex
+ for (i=0;i<numverts;i++)
+ {
+ if (m_simplexVectorW[i] == w)
+ found = true;
+ }
+
+ //check in case lastW is already removed
+ if (w == m_lastW)
+ return true;
+
+ return found;
+}
+
+void btVoronoiSimplexSolver::backup_closest(btVector3& v)
+{
+ v = m_cachedV;
+}
+
+
+bool btVoronoiSimplexSolver::emptySimplex() const
+{
+ return (numVertices() == 0);
+
+}
+
+void btVoronoiSimplexSolver::compute_points(btVector3& p1, btVector3& p2)
+{
+ updateClosestVectorAndPoints();
+ p1 = m_cachedP1;
+ p2 = m_cachedP2;
+
+}
+
+
+
+
+bool btVoronoiSimplexSolver::closestPtPointTriangle(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c,btSubSimplexClosestResult& result)
+{
+ result.m_usedVertices.reset();
+
+ // Check if P in vertex region outside A
+ btVector3 ab = b - a;
+ btVector3 ac = c - a;
+ btVector3 ap = p - a;
+ btScalar d1 = ab.dot(ap);
+ btScalar d2 = ac.dot(ap);
+ if (d1 <= btScalar(0.0) && d2 <= btScalar(0.0))
+ {
+ result.m_closestPointOnSimplex = a;
+ result.m_usedVertices.usedVertexA = true;
+ result.setBarycentricCoordinates(1,0,0);
+ return true;// a; // barycentric coordinates (1,0,0)
+ }
+
+ // Check if P in vertex region outside B
+ btVector3 bp = p - b;
+ btScalar d3 = ab.dot(bp);
+ btScalar d4 = ac.dot(bp);
+ if (d3 >= btScalar(0.0) && d4 <= d3)
+ {
+ result.m_closestPointOnSimplex = b;
+ result.m_usedVertices.usedVertexB = true;
+ result.setBarycentricCoordinates(0,1,0);
+
+ return true; // b; // barycentric coordinates (0,1,0)
+ }
+ // Check if P in edge region of AB, if so return projection of P onto AB
+ btScalar vc = d1*d4 - d3*d2;
+ if (vc <= btScalar(0.0) && d1 >= btScalar(0.0) && d3 <= btScalar(0.0)) {
+ btScalar v = d1 / (d1 - d3);
+ result.m_closestPointOnSimplex = a + v * ab;
+ result.m_usedVertices.usedVertexA = true;
+ result.m_usedVertices.usedVertexB = true;
+ result.setBarycentricCoordinates(1-v,v,0);
+ return true;
+ //return a + v * ab; // barycentric coordinates (1-v,v,0)
+ }
+
+ // Check if P in vertex region outside C
+ btVector3 cp = p - c;
+ btScalar d5 = ab.dot(cp);
+ btScalar d6 = ac.dot(cp);
+ if (d6 >= btScalar(0.0) && d5 <= d6)
+ {
+ result.m_closestPointOnSimplex = c;
+ result.m_usedVertices.usedVertexC = true;
+ result.setBarycentricCoordinates(0,0,1);
+ return true;//c; // barycentric coordinates (0,0,1)
+ }
+
+ // Check if P in edge region of AC, if so return projection of P onto AC
+ btScalar vb = d5*d2 - d1*d6;
+ if (vb <= btScalar(0.0) && d2 >= btScalar(0.0) && d6 <= btScalar(0.0)) {
+ btScalar w = d2 / (d2 - d6);
+ result.m_closestPointOnSimplex = a + w * ac;
+ result.m_usedVertices.usedVertexA = true;
+ result.m_usedVertices.usedVertexC = true;
+ result.setBarycentricCoordinates(1-w,0,w);
+ return true;
+ //return a + w * ac; // barycentric coordinates (1-w,0,w)
+ }
+
+ // Check if P in edge region of BC, if so return projection of P onto BC
+ btScalar va = d3*d6 - d5*d4;
+ if (va <= btScalar(0.0) && (d4 - d3) >= btScalar(0.0) && (d5 - d6) >= btScalar(0.0)) {
+ btScalar w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
+
+ result.m_closestPointOnSimplex = b + w * (c - b);
+ result.m_usedVertices.usedVertexB = true;
+ result.m_usedVertices.usedVertexC = true;
+ result.setBarycentricCoordinates(0,1-w,w);
+ return true;
+ // return b + w * (c - b); // barycentric coordinates (0,1-w,w)
+ }
+
+ // P inside face region. Compute Q through its barycentric coordinates (u,v,w)
+ btScalar denom = btScalar(1.0) / (va + vb + vc);
+ btScalar v = vb * denom;
+ btScalar w = vc * denom;
+
+ result.m_closestPointOnSimplex = a + ab * v + ac * w;
+ result.m_usedVertices.usedVertexA = true;
+ result.m_usedVertices.usedVertexB = true;
+ result.m_usedVertices.usedVertexC = true;
+ result.setBarycentricCoordinates(1-v-w,v,w);
+
+ return true;
+// return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = btScalar(1.0) - v - w
+
+}
+
+
+
+
+
+/// Test if point p and d lie on opposite sides of plane through abc
+int btVoronoiSimplexSolver::pointOutsideOfPlane(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d)
+{
+ btVector3 normal = (b-a).cross(c-a);
+
+ btScalar signp = (p - a).dot(normal); // [AP AB AC]
+ btScalar signd = (d - a).dot( normal); // [AD AB AC]
+
+#ifdef CATCH_DEGENERATE_TETRAHEDRON
+#ifdef BT_USE_DOUBLE_PRECISION
+if (signd * signd < (btScalar(1e-8) * btScalar(1e-8)))
+ {
+ return -1;
+ }
+#else
+ if (signd * signd < (btScalar(1e-4) * btScalar(1e-4)))
+ {
+// printf("affine dependent/degenerate\n");//
+ return -1;
+ }
+#endif
+
+#endif
+ // Points on opposite sides if expression signs are opposite
+ return signp * signd < btScalar(0.);
+}
+
+
+bool btVoronoiSimplexSolver::closestPtPointTetrahedron(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, btSubSimplexClosestResult& finalResult)
+{
+ btSubSimplexClosestResult tempResult;
+
+ // Start out assuming point inside all halfspaces, so closest to itself
+ finalResult.m_closestPointOnSimplex = p;
+ finalResult.m_usedVertices.reset();
+ finalResult.m_usedVertices.usedVertexA = true;
+ finalResult.m_usedVertices.usedVertexB = true;
+ finalResult.m_usedVertices.usedVertexC = true;
+ finalResult.m_usedVertices.usedVertexD = true;
+
+ int pointOutsideABC = pointOutsideOfPlane(p, a, b, c, d);
+ int pointOutsideACD = pointOutsideOfPlane(p, a, c, d, b);
+ int pointOutsideADB = pointOutsideOfPlane(p, a, d, b, c);
+ int pointOutsideBDC = pointOutsideOfPlane(p, b, d, c, a);
+
+ if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0)
+ {
+ finalResult.m_degenerate = true;
+ return false;
+ }
+
+ if (!pointOutsideABC && !pointOutsideACD && !pointOutsideADB && !pointOutsideBDC)
+ {
+ return false;
+ }
+
+
+ btScalar bestSqDist = FLT_MAX;
+ // If point outside face abc then compute closest point on abc
+ if (pointOutsideABC)
+ {
+ closestPtPointTriangle(p, a, b, c,tempResult);
+ btVector3 q = tempResult.m_closestPointOnSimplex;
+
+ btScalar sqDist = (q - p).dot( q - p);
+ // Update best closest point if (squared) distance is less than current best
+ if (sqDist < bestSqDist) {
+ bestSqDist = sqDist;
+ finalResult.m_closestPointOnSimplex = q;
+ //convert result bitmask!
+ finalResult.m_usedVertices.reset();
+ finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
+ finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexB;
+ finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC;
+ finalResult.setBarycentricCoordinates(
+ tempResult.m_barycentricCoords[VERTA],
+ tempResult.m_barycentricCoords[VERTB],
+ tempResult.m_barycentricCoords[VERTC],
+ 0
+ );
+
+ }
+ }
+
+
+ // Repeat test for face acd
+ if (pointOutsideACD)
+ {
+ closestPtPointTriangle(p, a, c, d,tempResult);
+ btVector3 q = tempResult.m_closestPointOnSimplex;
+ //convert result bitmask!
+
+ btScalar sqDist = (q - p).dot( q - p);
+ if (sqDist < bestSqDist)
+ {
+ bestSqDist = sqDist;
+ finalResult.m_closestPointOnSimplex = q;
+ finalResult.m_usedVertices.reset();
+ finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
+
+ finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexB;
+ finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexC;
+ finalResult.setBarycentricCoordinates(
+ tempResult.m_barycentricCoords[VERTA],
+ 0,
+ tempResult.m_barycentricCoords[VERTB],
+ tempResult.m_barycentricCoords[VERTC]
+ );
+
+ }
+ }
+ // Repeat test for face adb
+
+
+ if (pointOutsideADB)
+ {
+ closestPtPointTriangle(p, a, d, b,tempResult);
+ btVector3 q = tempResult.m_closestPointOnSimplex;
+ //convert result bitmask!
+
+ btScalar sqDist = (q - p).dot( q - p);
+ if (sqDist < bestSqDist)
+ {
+ bestSqDist = sqDist;
+ finalResult.m_closestPointOnSimplex = q;
+ finalResult.m_usedVertices.reset();
+ finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
+ finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexC;
+
+ finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB;
+ finalResult.setBarycentricCoordinates(
+ tempResult.m_barycentricCoords[VERTA],
+ tempResult.m_barycentricCoords[VERTC],
+ 0,
+ tempResult.m_barycentricCoords[VERTB]
+ );
+
+ }
+ }
+ // Repeat test for face bdc
+
+
+ if (pointOutsideBDC)
+ {
+ closestPtPointTriangle(p, b, d, c,tempResult);
+ btVector3 q = tempResult.m_closestPointOnSimplex;
+ //convert result bitmask!
+ btScalar sqDist = (q - p).dot( q - p);
+ if (sqDist < bestSqDist)
+ {
+ bestSqDist = sqDist;
+ finalResult.m_closestPointOnSimplex = q;
+ finalResult.m_usedVertices.reset();
+ //
+ finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexA;
+ finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC;
+ finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB;
+
+ finalResult.setBarycentricCoordinates(
+ 0,
+ tempResult.m_barycentricCoords[VERTA],
+ tempResult.m_barycentricCoords[VERTC],
+ tempResult.m_barycentricCoords[VERTB]
+ );
+
+ }
+ }
+
+ //help! we ended up full !
+
+ if (finalResult.m_usedVertices.usedVertexA &&
+ finalResult.m_usedVertices.usedVertexB &&
+ finalResult.m_usedVertices.usedVertexC &&
+ finalResult.m_usedVertices.usedVertexD)
+ {
+ return true;
+ }
+
+ return true;
+}
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/btWheelInfo.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/btWheelInfo.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/btWheelInfo.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/btWheelInfo.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,56 @@
+/*
+ * Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/
+ *
+ * Permission to use, copy, modify, distribute and sell this software
+ * and its documentation for any purpose is hereby granted without fee,
+ * provided that the above copyright notice appear in all copies.
+ * Erwin Coumans makes no representations about the suitability
+ * of this software for any purpose.
+ * It is provided "as is" without express or implied warranty.
+*/
+#include "BulletDynamics/Vehicle/btWheelInfo.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h" // for pointvelocity
+
+
+btScalar btWheelInfo::getSuspensionRestLength() const
+{
+
+ return m_suspensionRestLength1;
+
+}
+
+void btWheelInfo::updateWheel(const btRigidBody& chassis,RaycastInfo& raycastInfo)
+{
+ (void)raycastInfo;
+
+
+ if (m_raycastInfo.m_isInContact)
+
+ {
+ btScalar project= m_raycastInfo.m_contactNormalWS.dot( m_raycastInfo.m_wheelDirectionWS );
+ btVector3 chassis_velocity_at_contactPoint;
+ btVector3 relpos = m_raycastInfo.m_contactPointWS - chassis.getCenterOfMassPosition();
+ chassis_velocity_at_contactPoint = chassis.getVelocityInLocalPoint( relpos );
+ btScalar projVel = m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint );
+ if ( project >= btScalar(-0.1))
+ {
+ m_suspensionRelativeVelocity = btScalar(0.0);
+ m_clippedInvContactDotSuspension = btScalar(1.0) / btScalar(0.1);
+ }
+ else
+ {
+ btScalar inv = btScalar(-1.) / project;
+ m_suspensionRelativeVelocity = projVel * inv;
+ m_clippedInvContactDotSuspension = inv;
+ }
+
+ }
+
+ else // Not in contact : position wheel in a nice (rest length) position
+ {
+ m_raycastInfo.m_suspensionLength = this->getSuspensionRestLength();
+ m_suspensionRelativeVelocity = btScalar(0.0);
+ m_raycastInfo.m_contactNormalWS = -m_raycastInfo.m_wheelDirectionWS;
+ m_clippedInvContactDotSuspension = btScalar(1.0);
+ }
+}
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/gim_box_set.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/gim_box_set.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/gim_box_set.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/gim_box_set.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,182 @@
+
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman at yahoo.com
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of EITHER:
+ (1) The GNU Lesser General Public License as published by the Free
+ Software Foundation; either version 2.1 of the License, or (at
+ your option) any later version. The text of the GNU Lesser
+ General Public License is included with this library in the
+ file GIMPACT-LICENSE-LGPL.TXT.
+ (2) The BSD-style license that is included with this library in
+ the file GIMPACT-LICENSE-BSD.TXT.
+ (3) The zlib/libpng license that is included with this library in
+ the file GIMPACT-LICENSE-ZLIB.TXT.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
+ GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+
+
+#include "BulletCollision/Gimpact/gim_box_set.h"
+
+
+GUINT GIM_BOX_TREE::_calc_splitting_axis(
+ gim_array<GIM_AABB_DATA> & primitive_boxes, GUINT startIndex, GUINT endIndex)
+{
+ GUINT i;
+
+ btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.));
+ btVector3 variance(btScalar(0.),btScalar(0.),btScalar(0.));
+ GUINT numIndices = endIndex-startIndex;
+
+ for (i=startIndex;i<endIndex;i++)
+ {
+ btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
+ primitive_boxes[i].m_bound.m_min);
+ means+=center;
+ }
+ means *= (btScalar(1.)/(btScalar)numIndices);
+
+ for (i=startIndex;i<endIndex;i++)
+ {
+ btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
+ primitive_boxes[i].m_bound.m_min);
+ btVector3 diff2 = center-means;
+ diff2 = diff2 * diff2;
+ variance += diff2;
+ }
+ variance *= (btScalar(1.)/ ((btScalar)numIndices-1) );
+
+ return variance.maxAxis();
+}
+
+
+GUINT GIM_BOX_TREE::_sort_and_calc_splitting_index(
+ gim_array<GIM_AABB_DATA> & primitive_boxes, GUINT startIndex,
+ GUINT endIndex, GUINT splitAxis)
+{
+ GUINT i;
+ GUINT splitIndex =startIndex;
+ GUINT numIndices = endIndex - startIndex;
+
+ // average of centers
+ btScalar splitValue = 0.0f;
+ for (i=startIndex;i<endIndex;i++)
+ {
+ splitValue+= 0.5f*(primitive_boxes[i].m_bound.m_max[splitAxis] +
+ primitive_boxes[i].m_bound.m_min[splitAxis]);
+ }
+ splitValue /= (btScalar)numIndices;
+
+ //sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'.
+ for (i=startIndex;i<endIndex;i++)
+ {
+ btScalar center = 0.5f*(primitive_boxes[i].m_bound.m_max[splitAxis] +
+ primitive_boxes[i].m_bound.m_min[splitAxis]);
+ if (center > splitValue)
+ {
+ //swap
+ primitive_boxes.swap(i,splitIndex);
+ splitIndex++;
+ }
+ }
+
+ //if the splitIndex causes unbalanced trees, fix this by using the center in between startIndex and endIndex
+ //otherwise the tree-building might fail due to stack-overflows in certain cases.
+ //unbalanced1 is unsafe: it can cause stack overflows
+ //bool unbalanced1 = ((splitIndex==startIndex) || (splitIndex == (endIndex-1)));
+
+ //unbalanced2 should work too: always use center (perfect balanced trees)
+ //bool unbalanced2 = true;
+
+ //this should be safe too:
+ GUINT rangeBalancedIndices = numIndices/3;
+ bool unbalanced = ((splitIndex<=(startIndex+rangeBalancedIndices)) || (splitIndex >=(endIndex-1-rangeBalancedIndices)));
+
+ if (unbalanced)
+ {
+ splitIndex = startIndex+ (numIndices>>1);
+ }
+
+ btAssert(!((splitIndex==startIndex) || (splitIndex == (endIndex))));
+
+ return splitIndex;
+}
+
+
+void GIM_BOX_TREE::_build_sub_tree(gim_array<GIM_AABB_DATA> & primitive_boxes, GUINT startIndex, GUINT endIndex)
+{
+ GUINT current_index = m_num_nodes++;
+
+ btAssert((endIndex-startIndex)>0);
+
+ if((endIndex-startIndex) == 1) //we got a leaf
+ {
+ m_node_array[current_index].m_left = 0;
+ m_node_array[current_index].m_right = 0;
+ m_node_array[current_index].m_escapeIndex = 0;
+
+ m_node_array[current_index].m_bound = primitive_boxes[startIndex].m_bound;
+ m_node_array[current_index].m_data = primitive_boxes[startIndex].m_data;
+ return;
+ }
+
+ //configure inner node
+
+ GUINT splitIndex;
+
+ //calc this node bounding box
+ m_node_array[current_index].m_bound.invalidate();
+ for (splitIndex=startIndex;splitIndex<endIndex;splitIndex++)
+ {
+ m_node_array[current_index].m_bound.merge(primitive_boxes[splitIndex].m_bound);
+ }
+
+ //calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'.
+
+ //split axis
+ splitIndex = _calc_splitting_axis(primitive_boxes,startIndex,endIndex);
+
+ splitIndex = _sort_and_calc_splitting_index(
+ primitive_boxes,startIndex,endIndex,splitIndex);
+
+ //configure this inner node : the left node index
+ m_node_array[current_index].m_left = m_num_nodes;
+ //build left child tree
+ _build_sub_tree(primitive_boxes, startIndex, splitIndex );
+
+ //configure this inner node : the right node index
+ m_node_array[current_index].m_right = m_num_nodes;
+
+ //build right child tree
+ _build_sub_tree(primitive_boxes, splitIndex ,endIndex);
+
+ //configure this inner node : the escape index
+ m_node_array[current_index].m_escapeIndex = m_num_nodes - current_index;
+}
+
+//! stackless build tree
+void GIM_BOX_TREE::build_tree(
+ gim_array<GIM_AABB_DATA> & primitive_boxes)
+{
+ // initialize node count to 0
+ m_num_nodes = 0;
+ // allocate nodes
+ m_node_array.resize(primitive_boxes.size()*2);
+
+ _build_sub_tree(primitive_boxes, 0, primitive_boxes.size());
+}
+
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/gim_contact.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/gim_contact.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/gim_contact.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/gim_contact.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,146 @@
+
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman at yahoo.com
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of EITHER:
+ (1) The GNU Lesser General Public License as published by the Free
+ Software Foundation; either version 2.1 of the License, or (at
+ your option) any later version. The text of the GNU Lesser
+ General Public License is included with this library in the
+ file GIMPACT-LICENSE-LGPL.TXT.
+ (2) The BSD-style license that is included with this library in
+ the file GIMPACT-LICENSE-BSD.TXT.
+ (3) The zlib/libpng license that is included with this library in
+ the file GIMPACT-LICENSE-ZLIB.TXT.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
+ GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+
+#include "BulletCollision/Gimpact/gim_contact.h"
+
+#define MAX_COINCIDENT 8
+
+void gim_contact_array::merge_contacts(
+ const gim_contact_array & contacts, bool normal_contact_average)
+{
+ clear();
+
+ if(contacts.size()==1)
+ {
+ push_back(contacts.back());
+ return;
+ }
+
+ gim_array<GIM_RSORT_TOKEN> keycontacts(contacts.size());
+ keycontacts.resize(contacts.size(),false);
+
+ //fill key contacts
+
+ GUINT i;
+
+ for (i = 0;i<contacts.size() ;i++ )
+ {
+ keycontacts[i].m_key = contacts[i].calc_key_contact();
+ keycontacts[i].m_value = i;
+ }
+
+ //sort keys
+ gim_heap_sort(keycontacts.pointer(),keycontacts.size(),GIM_RSORT_TOKEN_COMPARATOR());
+
+ // Merge contacts
+
+ GUINT coincident_count=0;
+ btVector3 coincident_normals[MAX_COINCIDENT];
+
+ GUINT last_key = keycontacts[0].m_key;
+ GUINT key = 0;
+
+ push_back(contacts[keycontacts[0].m_value]);
+ GIM_CONTACT * pcontact = &back();
+
+
+
+ for( i=1;i<keycontacts.size();i++)
+ {
+ key = keycontacts[i].m_key;
+ const GIM_CONTACT * scontact = &contacts[keycontacts[i].m_value];
+
+ if(last_key == key)//same points
+ {
+ //merge contact
+ if(pcontact->m_depth - CONTACT_DIFF_EPSILON > scontact->m_depth)//)
+ {
+ *pcontact = *scontact;
+ coincident_count = 0;
+ }
+ else if(normal_contact_average)
+ {
+ if(btFabs(pcontact->m_depth - scontact->m_depth)<CONTACT_DIFF_EPSILON)
+ {
+ if(coincident_count<MAX_COINCIDENT)
+ {
+ coincident_normals[coincident_count] = scontact->m_normal;
+ coincident_count++;
+ }
+ }
+ }
+ }
+ else
+ {//add new contact
+
+ if(normal_contact_average && coincident_count>0)
+ {
+ pcontact->interpolate_normals(coincident_normals,coincident_count);
+ coincident_count = 0;
+ }
+
+ push_back(*scontact);
+ pcontact = &back();
+ }
+ last_key = key;
+ }
+}
+
+void gim_contact_array::merge_contacts_unique(const gim_contact_array & contacts)
+{
+ clear();
+
+ if(contacts.size()==1)
+ {
+ push_back(contacts.back());
+ return;
+ }
+
+ GIM_CONTACT average_contact = contacts.back();
+
+ for (GUINT i=1;i<contacts.size() ;i++ )
+ {
+ average_contact.m_point += contacts[i].m_point;
+ average_contact.m_normal += contacts[i].m_normal * contacts[i].m_depth;
+ }
+
+ //divide
+ GREAL divide_average = 1.0f/((GREAL)contacts.size());
+
+ average_contact.m_point *= divide_average;
+
+ average_contact.m_normal *= divide_average;
+
+ average_contact.m_depth = average_contact.m_normal.length();
+
+ average_contact.m_normal /= average_contact.m_depth;
+
+}
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/gim_memory.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/gim_memory.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/gim_memory.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/gim_memory.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,135 @@
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman at yahoo.com
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of EITHER:
+ (1) The GNU Lesser General Public License as published by the Free
+ Software Foundation; either version 2.1 of the License, or (at
+ your option) any later version. The text of the GNU Lesser
+ General Public License is included with this library in the
+ file GIMPACT-LICENSE-LGPL.TXT.
+ (2) The BSD-style license that is included with this library in
+ the file GIMPACT-LICENSE-BSD.TXT.
+ (3) The zlib/libpng license that is included with this library in
+ the file GIMPACT-LICENSE-ZLIB.TXT.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
+ GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+
+
+#include "BulletCollision/Gimpact/gim_memory.h"
+#include <stdlib.h>
+
+#ifdef GIM_SIMD_MEMORY
+#include "LinearMath/btAlignedAllocator.h"
+#endif
+
+static gim_alloc_function *g_allocfn = 0;
+static gim_alloca_function *g_allocafn = 0;
+static gim_realloc_function *g_reallocfn = 0;
+static gim_free_function *g_freefn = 0;
+
+void gim_set_alloc_handler (gim_alloc_function *fn)
+{
+ g_allocfn = fn;
+}
+
+void gim_set_alloca_handler (gim_alloca_function *fn)
+{
+ g_allocafn = fn;
+}
+
+void gim_set_realloc_handler (gim_realloc_function *fn)
+{
+ g_reallocfn = fn;
+}
+
+void gim_set_free_handler (gim_free_function *fn)
+{
+ g_freefn = fn;
+}
+
+gim_alloc_function *gim_get_alloc_handler()
+{
+ return g_allocfn;
+}
+
+gim_alloca_function *gim_get_alloca_handler()
+{
+ return g_allocafn;
+}
+
+
+gim_realloc_function *gim_get_realloc_handler ()
+{
+ return g_reallocfn;
+}
+
+
+gim_free_function *gim_get_free_handler ()
+{
+ return g_freefn;
+}
+
+
+void * gim_alloc(size_t size)
+{
+ void * ptr;
+ if (g_allocfn)
+ {
+ ptr = g_allocfn(size);
+ }
+ else
+ {
+#ifdef GIM_SIMD_MEMORY
+ ptr = btAlignedAlloc(size,16);
+#else
+ ptr = malloc(size);
+#endif
+ }
+ return ptr;
+}
+
+void * gim_alloca(size_t size)
+{
+ if (g_allocafn) return g_allocafn(size); else return gim_alloc(size);
+}
+
+
+void * gim_realloc(void *ptr, size_t oldsize, size_t newsize)
+{
+ void * newptr = gim_alloc(newsize);
+ size_t copysize = oldsize<newsize?oldsize:newsize;
+ gim_simd_memcpy(newptr,ptr,copysize);
+ gim_free(ptr);
+ return newptr;
+}
+
+void gim_free(void *ptr)
+{
+ if (!ptr) return;
+ if (g_freefn)
+ {
+ g_freefn(ptr);
+ }
+ else
+ {
+ #ifdef GIM_SIMD_MEMORY
+ btAlignedFree(ptr);
+ #else
+ free(ptr);
+ #endif
+ }
+}
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/gim_tri_collision.cpp
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/gim_tri_collision.cpp?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/gim_tri_collision.cpp (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/gim_tri_collision.cpp Sat Dec 19 14:05:59 2009
@@ -0,0 +1,640 @@
+
+/*! \file gim_tri_collision.h
+\author Francisco Len Nßjera
+*/
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman at yahoo.com
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of EITHER:
+ (1) The GNU Lesser General Public License as published by the Free
+ Software Foundation; either version 2.1 of the License, or (at
+ your option) any later version. The text of the GNU Lesser
+ General Public License is included with this library in the
+ file GIMPACT-LICENSE-LGPL.TXT.
+ (2) The BSD-style license that is included with this library in
+ the file GIMPACT-LICENSE-BSD.TXT.
+ (3) The zlib/libpng license that is included with this library in
+ the file GIMPACT-LICENSE-ZLIB.TXT.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
+ GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+
+#include "BulletCollision/Gimpact/gim_tri_collision.h"
+
+
+#define TRI_LOCAL_EPSILON 0.000001f
+#define MIN_EDGE_EDGE_DIS 0.00001f
+
+
+class GIM_TRIANGLE_CALCULATION_CACHE
+{
+public:
+ GREAL margin;
+ btVector3 tu_vertices[3];
+ btVector3 tv_vertices[3];
+ btVector4 tu_plane;
+ btVector4 tv_plane;
+ btVector3 closest_point_u;
+ btVector3 closest_point_v;
+ btVector3 edge_edge_dir;
+ btVector3 distances;
+ GREAL du[4];
+ GREAL du0du1;
+ GREAL du0du2;
+ GREAL dv[4];
+ GREAL dv0dv1;
+ GREAL dv0dv2;
+ btVector3 temp_points[MAX_TRI_CLIPPING];
+ btVector3 temp_points1[MAX_TRI_CLIPPING];
+ btVector3 contact_points[MAX_TRI_CLIPPING];
+
+
+
+ //! if returns false, the faces are paralele
+ SIMD_FORCE_INLINE bool compute_intervals(
+ const GREAL &D0,
+ const GREAL &D1,
+ const GREAL &D2,
+ const GREAL &D0D1,
+ const GREAL &D0D2,
+ GREAL & scale_edge0,
+ GREAL & scale_edge1,
+ GUINT &edge_index0,
+ GUINT &edge_index1)
+ {
+ if(D0D1>0.0f)
+ {
+ /* here we know that D0D2<=0.0 */
+ /* that is D0, D1 are on the same side, D2 on the other or on the plane */
+ scale_edge0 = -D2/(D0-D2);
+ scale_edge1 = -D1/(D2-D1);
+ edge_index0 = 2;edge_index1 = 1;
+ }
+ else if(D0D2>0.0f)
+ {
+ /* here we know that d0d1<=0.0 */
+ scale_edge0 = -D0/(D1-D0);
+ scale_edge1 = -D1/(D2-D1);
+ edge_index0 = 0;edge_index1 = 1;
+ }
+ else if(D1*D2>0.0f || D0!=0.0f)
+ {
+ /* here we know that d0d1<=0.0 or that D0!=0.0 */
+ scale_edge0 = -D0/(D1-D0);
+ scale_edge1 = -D2/(D0-D2);
+ edge_index0 = 0 ;edge_index1 = 2;
+ }
+ else
+ {
+ return false;
+ }
+ return true;
+ }
+
+
+ //! clip triangle
+ /*!
+ */
+ SIMD_FORCE_INLINE GUINT clip_triangle(
+ const btVector4 & tri_plane,
+ const btVector3 * tripoints,
+ const btVector3 * srcpoints,
+ btVector3 * clip_points)
+ {
+ // edge 0
+
+ btVector4 edgeplane;
+
+ EDGE_PLANE(tripoints[0],tripoints[1],tri_plane,edgeplane);
+
+ GUINT clipped_count = PLANE_CLIP_TRIANGLE3D(
+ edgeplane,srcpoints[0],srcpoints[1],srcpoints[2],temp_points);
+
+ if(clipped_count == 0) return 0;
+
+ // edge 1
+
+ EDGE_PLANE(tripoints[1],tripoints[2],tri_plane,edgeplane);
+
+ clipped_count = PLANE_CLIP_POLYGON3D(
+ edgeplane,temp_points,clipped_count,temp_points1);
+
+ if(clipped_count == 0) return 0;
+
+ // edge 2
+
+ EDGE_PLANE(tripoints[2],tripoints[0],tri_plane,edgeplane);
+
+ clipped_count = PLANE_CLIP_POLYGON3D(
+ edgeplane,temp_points1,clipped_count,clip_points);
+
+ return clipped_count;
+
+
+ /*GUINT i0 = (tri_plane.closestAxis()+1)%3;
+ GUINT i1 = (i0+1)%3;
+ // edge 0
+ btVector3 temp_points[MAX_TRI_CLIPPING];
+ btVector3 temp_points1[MAX_TRI_CLIPPING];
+
+ GUINT clipped_count= PLANE_CLIP_TRIANGLE_GENERIC(
+ 0,srcpoints[0],srcpoints[1],srcpoints[2],temp_points,
+ DISTANCE_EDGE(tripoints[0],tripoints[1],i0,i1));
+
+
+ if(clipped_count == 0) return 0;
+
+ // edge 1
+ clipped_count = PLANE_CLIP_POLYGON_GENERIC(
+ 0,temp_points,clipped_count,temp_points1,
+ DISTANCE_EDGE(tripoints[1],tripoints[2],i0,i1));
+
+ if(clipped_count == 0) return 0;
+
+ // edge 2
+ clipped_count = PLANE_CLIP_POLYGON_GENERIC(
+ 0,temp_points1,clipped_count,clipped_points,
+ DISTANCE_EDGE(tripoints[2],tripoints[0],i0,i1));
+
+ return clipped_count;*/
+ }
+
+ SIMD_FORCE_INLINE void sort_isect(
+ GREAL & isect0,GREAL & isect1,GUINT &e0,GUINT &e1,btVector3 & vec0,btVector3 & vec1)
+ {
+ if(isect1<isect0)
+ {
+ //swap
+ GIM_SWAP_NUMBERS(isect0,isect1);
+ GIM_SWAP_NUMBERS(e0,e1);
+ btVector3 tmp = vec0;
+ vec0 = vec1;
+ vec1 = tmp;
+ }
+ }
+
+ //! Test verifying interval intersection with the direction between planes
+ /*!
+ \pre tv_plane and tu_plane must be set
+ \post
+ distances[2] is set with the distance
+ closest_point_u, closest_point_v, edge_edge_dir are set too
+ \return
+ - 0: faces are paralele
+ - 1: face U casts face V
+ - 2: face V casts face U
+ - 3: nearest edges
+ */
+ SIMD_FORCE_INLINE GUINT cross_line_intersection_test()
+ {
+ // Compute direction of intersection line
+ edge_edge_dir = tu_plane.cross(tv_plane);
+ GREAL Dlen;
+ VEC_LENGTH(edge_edge_dir,Dlen);
+
+ if(Dlen<0.0001)
+ {
+ return 0; //faces near paralele
+ }
+
+ edge_edge_dir*= 1/Dlen;//normalize
+
+
+ // Compute interval for triangle 1
+ GUINT tu_e0,tu_e1;//edge indices
+ GREAL tu_scale_e0,tu_scale_e1;//edge scale
+ if(!compute_intervals(du[0],du[1],du[2],
+ du0du1,du0du2,tu_scale_e0,tu_scale_e1,tu_e0,tu_e1)) return 0;
+
+ // Compute interval for triangle 2
+ GUINT tv_e0,tv_e1;//edge indices
+ GREAL tv_scale_e0,tv_scale_e1;//edge scale
+
+ if(!compute_intervals(dv[0],dv[1],dv[2],
+ dv0dv1,dv0dv2,tv_scale_e0,tv_scale_e1,tv_e0,tv_e1)) return 0;
+
+ //proyected vertices
+ btVector3 up_e0 = tu_vertices[tu_e0].lerp(tu_vertices[(tu_e0+1)%3],tu_scale_e0);
+ btVector3 up_e1 = tu_vertices[tu_e1].lerp(tu_vertices[(tu_e1+1)%3],tu_scale_e1);
+
+ btVector3 vp_e0 = tv_vertices[tv_e0].lerp(tv_vertices[(tv_e0+1)%3],tv_scale_e0);
+ btVector3 vp_e1 = tv_vertices[tv_e1].lerp(tv_vertices[(tv_e1+1)%3],tv_scale_e1);
+
+ //proyected intervals
+ GREAL isect_u[] = {up_e0.dot(edge_edge_dir),up_e1.dot(edge_edge_dir)};
+ GREAL isect_v[] = {vp_e0.dot(edge_edge_dir),vp_e1.dot(edge_edge_dir)};
+
+ sort_isect(isect_u[0],isect_u[1],tu_e0,tu_e1,up_e0,up_e1);
+ sort_isect(isect_v[0],isect_v[1],tv_e0,tv_e1,vp_e0,vp_e1);
+
+ const GREAL midpoint_u = 0.5f*(isect_u[0]+isect_u[1]); // midpoint
+ const GREAL midpoint_v = 0.5f*(isect_v[0]+isect_v[1]); // midpoint
+
+ if(midpoint_u<midpoint_v)
+ {
+ if(isect_u[1]>=isect_v[1]) // face U casts face V
+ {
+ return 1;
+ }
+ else if(isect_v[0]<=isect_u[0]) // face V casts face U
+ {
+ return 2;
+ }
+ // closest points
+ closest_point_u = up_e1;
+ closest_point_v = vp_e0;
+ // calc edges and separation
+
+ if(isect_u[1]+ MIN_EDGE_EDGE_DIS<isect_v[0]) //calc distance between two lines instead
+ {
+ SEGMENT_COLLISION(
+ tu_vertices[tu_e1],tu_vertices[(tu_e1+1)%3],
+ tv_vertices[tv_e0],tv_vertices[(tv_e0+1)%3],
+ closest_point_u,
+ closest_point_v);
+
+ edge_edge_dir = closest_point_u-closest_point_v;
+ VEC_LENGTH(edge_edge_dir,distances[2]);
+ edge_edge_dir *= 1.0f/distances[2];// normalize
+ }
+ else
+ {
+ distances[2] = isect_v[0]-isect_u[1];//distance negative
+ //edge_edge_dir *= -1.0f; //normal pointing from V to U
+ }
+
+ }
+ else
+ {
+ if(isect_v[1]>=isect_u[1]) // face V casts face U
+ {
+ return 2;
+ }
+ else if(isect_u[0]<=isect_v[0]) // face U casts face V
+ {
+ return 1;
+ }
+ // closest points
+ closest_point_u = up_e0;
+ closest_point_v = vp_e1;
+ // calc edges and separation
+
+ if(isect_v[1]+MIN_EDGE_EDGE_DIS<isect_u[0]) //calc distance between two lines instead
+ {
+ SEGMENT_COLLISION(
+ tu_vertices[tu_e0],tu_vertices[(tu_e0+1)%3],
+ tv_vertices[tv_e1],tv_vertices[(tv_e1+1)%3],
+ closest_point_u,
+ closest_point_v);
+
+ edge_edge_dir = closest_point_u-closest_point_v;
+ VEC_LENGTH(edge_edge_dir,distances[2]);
+ edge_edge_dir *= 1.0f/distances[2];// normalize
+ }
+ else
+ {
+ distances[2] = isect_u[0]-isect_v[1];//distance negative
+ //edge_edge_dir *= -1.0f; //normal pointing from V to U
+ }
+ }
+ return 3;
+ }
+
+
+ //! collides by two sides
+ SIMD_FORCE_INLINE bool triangle_collision(
+ const btVector3 & u0,
+ const btVector3 & u1,
+ const btVector3 & u2,
+ GREAL margin_u,
+ const btVector3 & v0,
+ const btVector3 & v1,
+ const btVector3 & v2,
+ GREAL margin_v,
+ GIM_TRIANGLE_CONTACT_DATA & contacts)
+ {
+
+ margin = margin_u + margin_v;
+
+ tu_vertices[0] = u0;
+ tu_vertices[1] = u1;
+ tu_vertices[2] = u2;
+
+ tv_vertices[0] = v0;
+ tv_vertices[1] = v1;
+ tv_vertices[2] = v2;
+
+ //create planes
+ // plane v vs U points
+
+ TRIANGLE_PLANE(tv_vertices[0],tv_vertices[1],tv_vertices[2],tv_plane);
+
+ du[0] = DISTANCE_PLANE_POINT(tv_plane,tu_vertices[0]);
+ du[1] = DISTANCE_PLANE_POINT(tv_plane,tu_vertices[1]);
+ du[2] = DISTANCE_PLANE_POINT(tv_plane,tu_vertices[2]);
+
+
+ du0du1 = du[0] * du[1];
+ du0du2 = du[0] * du[2];
+
+
+ if(du0du1>0.0f && du0du2>0.0f) // same sign on all of them + not equal 0 ?
+ {
+ if(du[0]<0) //we need test behind the triangle plane
+ {
+ distances[0] = GIM_MAX3(du[0],du[1],du[2]);
+ distances[0] = -distances[0];
+ if(distances[0]>margin) return false; //never intersect
+
+ //reorder triangle v
+ VEC_SWAP(tv_vertices[0],tv_vertices[1]);
+ VEC_SCALE_4(tv_plane,-1.0f,tv_plane);
+ }
+ else
+ {
+ distances[0] = GIM_MIN3(du[0],du[1],du[2]);
+ if(distances[0]>margin) return false; //never intersect
+ }
+ }
+ else
+ {
+ //Look if we need to invert the triangle
+ distances[0] = (du[0]+du[1]+du[2])/3.0f; //centroid
+
+ if(distances[0]<0.0f)
+ {
+ //reorder triangle v
+ VEC_SWAP(tv_vertices[0],tv_vertices[1]);
+ VEC_SCALE_4(tv_plane,-1.0f,tv_plane);
+
+ distances[0] = GIM_MAX3(du[0],du[1],du[2]);
+ distances[0] = -distances[0];
+ }
+ else
+ {
+ distances[0] = GIM_MIN3(du[0],du[1],du[2]);
+ }
+ }
+
+
+ // plane U vs V points
+
+ TRIANGLE_PLANE(tu_vertices[0],tu_vertices[1],tu_vertices[2],tu_plane);
+
+ dv[0] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[0]);
+ dv[1] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[1]);
+ dv[2] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[2]);
+
+ dv0dv1 = dv[0] * dv[1];
+ dv0dv2 = dv[0] * dv[2];
+
+
+ if(dv0dv1>0.0f && dv0dv2>0.0f) // same sign on all of them + not equal 0 ?
+ {
+ if(dv[0]<0) //we need test behind the triangle plane
+ {
+ distances[1] = GIM_MAX3(dv[0],dv[1],dv[2]);
+ distances[1] = -distances[1];
+ if(distances[1]>margin) return false; //never intersect
+
+ //reorder triangle u
+ VEC_SWAP(tu_vertices[0],tu_vertices[1]);
+ VEC_SCALE_4(tu_plane,-1.0f,tu_plane);
+ }
+ else
+ {
+ distances[1] = GIM_MIN3(dv[0],dv[1],dv[2]);
+ if(distances[1]>margin) return false; //never intersect
+ }
+ }
+ else
+ {
+ //Look if we need to invert the triangle
+ distances[1] = (dv[0]+dv[1]+dv[2])/3.0f; //centroid
+
+ if(distances[1]<0.0f)
+ {
+ //reorder triangle v
+ VEC_SWAP(tu_vertices[0],tu_vertices[1]);
+ VEC_SCALE_4(tu_plane,-1.0f,tu_plane);
+
+ distances[1] = GIM_MAX3(dv[0],dv[1],dv[2]);
+ distances[1] = -distances[1];
+ }
+ else
+ {
+ distances[1] = GIM_MIN3(dv[0],dv[1],dv[2]);
+ }
+ }
+
+ GUINT bl;
+ /* bl = cross_line_intersection_test();
+ if(bl==3)
+ {
+ //take edge direction too
+ bl = distances.maxAxis();
+ }
+ else
+ {*/
+ bl = 0;
+ if(distances[0]<distances[1]) bl = 1;
+ //}
+
+ if(bl==2) //edge edge separation
+ {
+ if(distances[2]>margin) return false;
+
+ contacts.m_penetration_depth = -distances[2] + margin;
+ contacts.m_points[0] = closest_point_v;
+ contacts.m_point_count = 1;
+ VEC_COPY(contacts.m_separating_normal,edge_edge_dir);
+
+ return true;
+ }
+
+ //clip face against other
+
+
+ GUINT point_count;
+ //TODO
+ if(bl == 0) //clip U points against V
+ {
+ point_count = clip_triangle(tv_plane,tv_vertices,tu_vertices,contact_points);
+ if(point_count == 0) return false;
+ contacts.merge_points(tv_plane,margin,contact_points,point_count);
+ }
+ else //clip V points against U
+ {
+ point_count = clip_triangle(tu_plane,tu_vertices,tv_vertices,contact_points);
+ if(point_count == 0) return false;
+ contacts.merge_points(tu_plane,margin,contact_points,point_count);
+ contacts.m_separating_normal *= -1.f;
+ }
+ if(contacts.m_point_count == 0) return false;
+ return true;
+ }
+
+};
+
+
+/*class GIM_TRIANGLE_CALCULATION_CACHE
+{
+public:
+ GREAL margin;
+ GUINT clipped_count;
+ btVector3 tu_vertices[3];
+ btVector3 tv_vertices[3];
+ btVector3 temp_points[MAX_TRI_CLIPPING];
+ btVector3 temp_points1[MAX_TRI_CLIPPING];
+ btVector3 clipped_points[MAX_TRI_CLIPPING];
+ GIM_TRIANGLE_CONTACT_DATA contacts1;
+ GIM_TRIANGLE_CONTACT_DATA contacts2;
+
+
+ //! clip triangle
+ GUINT clip_triangle(
+ const btVector4 & tri_plane,
+ const btVector3 * tripoints,
+ const btVector3 * srcpoints,
+ btVector3 * clipped_points)
+ {
+ // edge 0
+
+ btVector4 edgeplane;
+
+ EDGE_PLANE(tripoints[0],tripoints[1],tri_plane,edgeplane);
+
+ GUINT clipped_count = PLANE_CLIP_TRIANGLE3D(
+ edgeplane,srcpoints[0],srcpoints[1],srcpoints[2],temp_points);
+
+ if(clipped_count == 0) return 0;
+
+ // edge 1
+
+ EDGE_PLANE(tripoints[1],tripoints[2],tri_plane,edgeplane);
+
+ clipped_count = PLANE_CLIP_POLYGON3D(
+ edgeplane,temp_points,clipped_count,temp_points1);
+
+ if(clipped_count == 0) return 0;
+
+ // edge 2
+
+ EDGE_PLANE(tripoints[2],tripoints[0],tri_plane,edgeplane);
+
+ clipped_count = PLANE_CLIP_POLYGON3D(
+ edgeplane,temp_points1,clipped_count,clipped_points);
+
+ return clipped_count;
+ }
+
+
+
+
+ //! collides only on one side
+ bool triangle_collision(
+ const btVector3 & u0,
+ const btVector3 & u1,
+ const btVector3 & u2,
+ GREAL margin_u,
+ const btVector3 & v0,
+ const btVector3 & v1,
+ const btVector3 & v2,
+ GREAL margin_v,
+ GIM_TRIANGLE_CONTACT_DATA & contacts)
+ {
+
+ margin = margin_u + margin_v;
+
+
+ tu_vertices[0] = u0;
+ tu_vertices[1] = u1;
+ tu_vertices[2] = u2;
+
+ tv_vertices[0] = v0;
+ tv_vertices[1] = v1;
+ tv_vertices[2] = v2;
+
+ //create planes
+ // plane v vs U points
+
+
+ TRIANGLE_PLANE(tv_vertices[0],tv_vertices[1],tv_vertices[2],contacts1.m_separating_normal);
+
+ clipped_count = clip_triangle(
+ contacts1.m_separating_normal,tv_vertices,tu_vertices,clipped_points);
+
+ if(clipped_count == 0 )
+ {
+ return false;//Reject
+ }
+
+ //find most deep interval face1
+ contacts1.merge_points(contacts1.m_separating_normal,margin,clipped_points,clipped_count);
+ if(contacts1.m_point_count == 0) return false; // too far
+
+ //Normal pointing to triangle1
+ //contacts1.m_separating_normal *= -1.f;
+
+ //Clip tri1 by tri2 edges
+
+ TRIANGLE_PLANE(tu_vertices[0],tu_vertices[1],tu_vertices[2],contacts2.m_separating_normal);
+
+ clipped_count = clip_triangle(
+ contacts2.m_separating_normal,tu_vertices,tv_vertices,clipped_points);
+
+ if(clipped_count == 0 )
+ {
+ return false;//Reject
+ }
+
+ //find most deep interval face1
+ contacts2.merge_points(contacts2.m_separating_normal,margin,clipped_points,clipped_count);
+ if(contacts2.m_point_count == 0) return false; // too far
+
+ contacts2.m_separating_normal *= -1.f;
+
+ ////check most dir for contacts
+ if(contacts2.m_penetration_depth<contacts1.m_penetration_depth)
+ {
+ contacts.copy_from(contacts2);
+ }
+ else
+ {
+ contacts.copy_from(contacts1);
+ }
+ return true;
+ }
+
+
+};*/
+
+
+
+bool GIM_TRIANGLE::collide_triangle_hard_test(
+ const GIM_TRIANGLE & other,
+ GIM_TRIANGLE_CONTACT_DATA & contact_data) const
+{
+ GIM_TRIANGLE_CALCULATION_CACHE calc_cache;
+ return calc_cache.triangle_collision(
+ m_vertices[0],m_vertices[1],m_vertices[2],m_margin,
+ other.m_vertices[0],other.m_vertices[1],other.m_vertices[2],other.m_margin,
+ contact_data);
+
+}
+
+
+
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btAxisSweep3.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btAxisSweep3.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btAxisSweep3.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btAxisSweep3.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,1024 @@
+//Bullet Continuous Collision Detection and Physics Library
+//Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+//
+// btAxisSweep3.h
+//
+// Copyright (c) 2006 Simon Hobbs
+//
+// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
+//
+// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
+//
+// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+//
+// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+//
+// 3. This notice may not be removed or altered from any source distribution.
+
+#ifndef AXIS_SWEEP_3_H
+#define AXIS_SWEEP_3_H
+
+#include "LinearMath/btVector3.h"
+#include "btOverlappingPairCache.h"
+#include "btBroadphaseInterface.h"
+#include "btBroadphaseProxy.h"
+#include "btOverlappingPairCallback.h"
+#include "btDbvtBroadphase.h"
+
+//#define DEBUG_BROADPHASE 1
+#define USE_OVERLAP_TEST_ON_REMOVES 1
+
+/// The internal templace class btAxisSweep3Internal implements the sweep and prune broadphase.
+/// It uses quantized integers to represent the begin and end points for each of the 3 axis.
+/// Dont use this class directly, use btAxisSweep3 or bt32BitAxisSweep3 instead.
+template <typename BP_FP_INT_TYPE>
+class btAxisSweep3Internal : public btBroadphaseInterface
+{
+protected:
+
+ BP_FP_INT_TYPE m_bpHandleMask;
+ BP_FP_INT_TYPE m_handleSentinel;
+
+public:
+
+ BT_DECLARE_ALIGNED_ALLOCATOR();
+
+ class Edge
+ {
+ public:
+ BP_FP_INT_TYPE m_pos; // low bit is min/max
+ BP_FP_INT_TYPE m_handle;
+
+ BP_FP_INT_TYPE IsMax() const {return static_cast<BP_FP_INT_TYPE>(m_pos & 1);}
+ };
+
+public:
+ class Handle : public btBroadphaseProxy
+ {
+ public:
+ BT_DECLARE_ALIGNED_ALLOCATOR();
+
+ // indexes into the edge arrays
+ BP_FP_INT_TYPE m_minEdges[3], m_maxEdges[3]; // 6 * 2 = 12
+// BP_FP_INT_TYPE m_uniqueId;
+ btBroadphaseProxy* m_dbvtProxy;//for faster raycast
+ //void* m_pOwner; this is now in btBroadphaseProxy.m_clientObject
+
+ SIMD_FORCE_INLINE void SetNextFree(BP_FP_INT_TYPE next) {m_minEdges[0] = next;}
+ SIMD_FORCE_INLINE BP_FP_INT_TYPE GetNextFree() const {return m_minEdges[0];}
+ }; // 24 bytes + 24 for Edge structures = 44 bytes total per entry
+
+
+protected:
+ btVector3 m_worldAabbMin; // overall system bounds
+ btVector3 m_worldAabbMax; // overall system bounds
+
+ btVector3 m_quantize; // scaling factor for quantization
+
+ BP_FP_INT_TYPE m_numHandles; // number of active handles
+ BP_FP_INT_TYPE m_maxHandles; // max number of handles
+ Handle* m_pHandles; // handles pool
+
+ BP_FP_INT_TYPE m_firstFreeHandle; // free handles list
+
+ Edge* m_pEdges[3]; // edge arrays for the 3 axes (each array has m_maxHandles * 2 + 2 sentinel entries)
+ void* m_pEdgesRawPtr[3];
+
+ btOverlappingPairCache* m_pairCache;
+
+ ///btOverlappingPairCallback is an additional optional user callback for adding/removing overlapping pairs, similar interface to btOverlappingPairCache.
+ btOverlappingPairCallback* m_userPairCallback;
+
+ bool m_ownsPairCache;
+
+ int m_invalidPair;
+
+ ///additional dynamic aabb structure, used to accelerate ray cast queries.
+ ///can be disabled using a optional argument in the constructor
+ btDbvtBroadphase* m_raycastAccelerator;
+ btOverlappingPairCache* m_nullPairCache;
+
+
+ // allocation/deallocation
+ BP_FP_INT_TYPE allocHandle();
+ void freeHandle(BP_FP_INT_TYPE handle);
+
+
+ bool testOverlap2D(const Handle* pHandleA, const Handle* pHandleB,int axis0,int axis1);
+
+#ifdef DEBUG_BROADPHASE
+ void debugPrintAxis(int axis,bool checkCardinality=true);
+#endif //DEBUG_BROADPHASE
+
+ //Overlap* AddOverlap(BP_FP_INT_TYPE handleA, BP_FP_INT_TYPE handleB);
+ //void RemoveOverlap(BP_FP_INT_TYPE handleA, BP_FP_INT_TYPE handleB);
+
+
+
+ void sortMinDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps );
+ void sortMinUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps );
+ void sortMaxDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps );
+ void sortMaxUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps );
+
+public:
+
+ btAxisSweep3Internal(const btVector3& worldAabbMin,const btVector3& worldAabbMax, BP_FP_INT_TYPE handleMask, BP_FP_INT_TYPE handleSentinel, BP_FP_INT_TYPE maxHandles = 16384, btOverlappingPairCache* pairCache=0,bool disableRaycastAccelerator = false);
+
+ virtual ~btAxisSweep3Internal();
+
+ BP_FP_INT_TYPE getNumHandles() const
+ {
+ return m_numHandles;
+ }
+
+ virtual void calculateOverlappingPairs(btDispatcher* dispatcher);
+
+ BP_FP_INT_TYPE addHandle(const btVector3& aabbMin,const btVector3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy);
+ void removeHandle(BP_FP_INT_TYPE handle,btDispatcher* dispatcher);
+ void updateHandle(BP_FP_INT_TYPE handle, const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher);
+ SIMD_FORCE_INLINE Handle* getHandle(BP_FP_INT_TYPE index) const {return m_pHandles + index;}
+
+ virtual void resetPool(btDispatcher* dispatcher);
+
+ void processAllOverlappingPairs(btOverlapCallback* callback);
+
+ //Broadphase Interface
+ virtual btBroadphaseProxy* createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy);
+ virtual void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
+ virtual void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher);
+ virtual void getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const;
+
+ virtual void rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin=btVector3(0,0,0), const btVector3& aabbMax = btVector3(0,0,0));
+
+ void quantize(BP_FP_INT_TYPE* out, const btVector3& point, int isMax) const;
+ ///unQuantize should be conservative: aabbMin/aabbMax should be larger then 'getAabb' result
+ void unQuantize(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const;
+
+ bool testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
+
+ btOverlappingPairCache* getOverlappingPairCache()
+ {
+ return m_pairCache;
+ }
+ const btOverlappingPairCache* getOverlappingPairCache() const
+ {
+ return m_pairCache;
+ }
+
+ void setOverlappingPairUserCallback(btOverlappingPairCallback* pairCallback)
+ {
+ m_userPairCallback = pairCallback;
+ }
+ const btOverlappingPairCallback* getOverlappingPairUserCallback() const
+ {
+ return m_userPairCallback;
+ }
+
+ ///getAabb returns the axis aligned bounding box in the 'global' coordinate frame
+ ///will add some transform later
+ virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const
+ {
+ aabbMin = m_worldAabbMin;
+ aabbMax = m_worldAabbMax;
+ }
+
+ virtual void printStats()
+ {
+/* printf("btAxisSweep3.h\n");
+ printf("numHandles = %d, maxHandles = %d\n",m_numHandles,m_maxHandles);
+ printf("aabbMin=%f,%f,%f,aabbMax=%f,%f,%f\n",m_worldAabbMin.getX(),m_worldAabbMin.getY(),m_worldAabbMin.getZ(),
+ m_worldAabbMax.getX(),m_worldAabbMax.getY(),m_worldAabbMax.getZ());
+ */
+
+ }
+
+};
+
+////////////////////////////////////////////////////////////////////
+
+
+
+
+#ifdef DEBUG_BROADPHASE
+#include <stdio.h>
+
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3<BP_FP_INT_TYPE>::debugPrintAxis(int axis, bool checkCardinality)
+{
+ int numEdges = m_pHandles[0].m_maxEdges[axis];
+ printf("SAP Axis %d, numEdges=%d\n",axis,numEdges);
+
+ int i;
+ for (i=0;i<numEdges+1;i++)
+ {
+ Edge* pEdge = m_pEdges[axis] + i;
+ Handle* pHandlePrev = getHandle(pEdge->m_handle);
+ int handleIndex = pEdge->IsMax()? pHandlePrev->m_maxEdges[axis] : pHandlePrev->m_minEdges[axis];
+ char beginOrEnd;
+ beginOrEnd=pEdge->IsMax()?'E':'B';
+ printf(" [%c,h=%d,p=%x,i=%d]\n",beginOrEnd,pEdge->m_handle,pEdge->m_pos,handleIndex);
+ }
+
+ if (checkCardinality)
+ btAssert(numEdges == m_numHandles*2+1);
+}
+#endif //DEBUG_BROADPHASE
+
+template <typename BP_FP_INT_TYPE>
+btBroadphaseProxy* btAxisSweep3Internal<BP_FP_INT_TYPE>::createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy)
+{
+ (void)shapeType;
+ BP_FP_INT_TYPE handleId = addHandle(aabbMin,aabbMax, userPtr,collisionFilterGroup,collisionFilterMask,dispatcher,multiSapProxy);
+
+ Handle* handle = getHandle(handleId);
+
+ if (m_raycastAccelerator)
+ {
+ btBroadphaseProxy* rayProxy = m_raycastAccelerator->createProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask,dispatcher,0);
+ handle->m_dbvtProxy = rayProxy;
+ }
+ return handle;
+}
+
+
+
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher)
+{
+ Handle* handle = static_cast<Handle*>(proxy);
+ if (m_raycastAccelerator)
+ m_raycastAccelerator->destroyProxy(handle->m_dbvtProxy,dispatcher);
+ removeHandle(static_cast<BP_FP_INT_TYPE>(handle->m_uniqueId), dispatcher);
+}
+
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher)
+{
+ Handle* handle = static_cast<Handle*>(proxy);
+ handle->m_aabbMin = aabbMin;
+ handle->m_aabbMax = aabbMax;
+ updateHandle(static_cast<BP_FP_INT_TYPE>(handle->m_uniqueId), aabbMin, aabbMax,dispatcher);
+ if (m_raycastAccelerator)
+ m_raycastAccelerator->setAabb(handle->m_dbvtProxy,aabbMin,aabbMax,dispatcher);
+
+}
+
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback,const btVector3& aabbMin,const btVector3& aabbMax)
+{
+ if (m_raycastAccelerator)
+ {
+ m_raycastAccelerator->rayTest(rayFrom,rayTo,rayCallback,aabbMin,aabbMax);
+ } else
+ {
+ //choose axis?
+ BP_FP_INT_TYPE axis = 0;
+ //for each proxy
+ for (BP_FP_INT_TYPE i=1;i<m_numHandles*2+1;i++)
+ {
+ if (m_pEdges[axis][i].IsMax())
+ {
+ rayCallback.process(getHandle(m_pEdges[axis][i].m_handle));
+ }
+ }
+ }
+}
+
+
+
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const
+{
+ Handle* pHandle = static_cast<Handle*>(proxy);
+ aabbMin = pHandle->m_aabbMin;
+ aabbMax = pHandle->m_aabbMax;
+}
+
+
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::unQuantize(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const
+{
+ Handle* pHandle = static_cast<Handle*>(proxy);
+
+ unsigned short vecInMin[3];
+ unsigned short vecInMax[3];
+
+ vecInMin[0] = m_pEdges[0][pHandle->m_minEdges[0]].m_pos ;
+ vecInMax[0] = m_pEdges[0][pHandle->m_maxEdges[0]].m_pos +1 ;
+ vecInMin[1] = m_pEdges[1][pHandle->m_minEdges[1]].m_pos ;
+ vecInMax[1] = m_pEdges[1][pHandle->m_maxEdges[1]].m_pos +1 ;
+ vecInMin[2] = m_pEdges[2][pHandle->m_minEdges[2]].m_pos ;
+ vecInMax[2] = m_pEdges[2][pHandle->m_maxEdges[2]].m_pos +1 ;
+
+ aabbMin.setValue((btScalar)(vecInMin[0]) / (m_quantize.getX()),(btScalar)(vecInMin[1]) / (m_quantize.getY()),(btScalar)(vecInMin[2]) / (m_quantize.getZ()));
+ aabbMin += m_worldAabbMin;
+
+ aabbMax.setValue((btScalar)(vecInMax[0]) / (m_quantize.getX()),(btScalar)(vecInMax[1]) / (m_quantize.getY()),(btScalar)(vecInMax[2]) / (m_quantize.getZ()));
+ aabbMax += m_worldAabbMin;
+}
+
+
+
+
+template <typename BP_FP_INT_TYPE>
+btAxisSweep3Internal<BP_FP_INT_TYPE>::btAxisSweep3Internal(const btVector3& worldAabbMin,const btVector3& worldAabbMax, BP_FP_INT_TYPE handleMask, BP_FP_INT_TYPE handleSentinel,BP_FP_INT_TYPE userMaxHandles, btOverlappingPairCache* pairCache , bool disableRaycastAccelerator)
+:m_bpHandleMask(handleMask),
+m_handleSentinel(handleSentinel),
+m_pairCache(pairCache),
+m_userPairCallback(0),
+m_ownsPairCache(false),
+m_invalidPair(0),
+m_raycastAccelerator(0)
+{
+ BP_FP_INT_TYPE maxHandles = static_cast<BP_FP_INT_TYPE>(userMaxHandles+1);//need to add one sentinel handle
+
+ if (!m_pairCache)
+ {
+ void* ptr = btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16);
+ m_pairCache = new(ptr) btHashedOverlappingPairCache();
+ m_ownsPairCache = true;
+ }
+
+ if (!disableRaycastAccelerator)
+ {
+ m_nullPairCache = new (btAlignedAlloc(sizeof(btNullPairCache),16)) btNullPairCache();
+ m_raycastAccelerator = new (btAlignedAlloc(sizeof(btDbvtBroadphase),16)) btDbvtBroadphase(m_nullPairCache);//m_pairCache);
+ m_raycastAccelerator->m_deferedcollide = true;//don't add/remove pairs
+ }
+
+ //btAssert(bounds.HasVolume());
+
+ // init bounds
+ m_worldAabbMin = worldAabbMin;
+ m_worldAabbMax = worldAabbMax;
+
+ btVector3 aabbSize = m_worldAabbMax - m_worldAabbMin;
+
+ BP_FP_INT_TYPE maxInt = m_handleSentinel;
+
+ m_quantize = btVector3(btScalar(maxInt),btScalar(maxInt),btScalar(maxInt)) / aabbSize;
+
+ // allocate handles buffer, using btAlignedAlloc, and put all handles on free list
+ m_pHandles = new Handle[maxHandles];
+
+ m_maxHandles = maxHandles;
+ m_numHandles = 0;
+
+ // handle 0 is reserved as the null index, and is also used as the sentinel
+ m_firstFreeHandle = 1;
+ {
+ for (BP_FP_INT_TYPE i = m_firstFreeHandle; i < maxHandles; i++)
+ m_pHandles[i].SetNextFree(static_cast<BP_FP_INT_TYPE>(i + 1));
+ m_pHandles[maxHandles - 1].SetNextFree(0);
+ }
+
+ {
+ // allocate edge buffers
+ for (int i = 0; i < 3; i++)
+ {
+ m_pEdgesRawPtr[i] = btAlignedAlloc(sizeof(Edge)*maxHandles*2,16);
+ m_pEdges[i] = new(m_pEdgesRawPtr[i]) Edge[maxHandles * 2];
+ }
+ }
+ //removed overlap management
+
+ // make boundary sentinels
+
+ m_pHandles[0].m_clientObject = 0;
+
+ for (int axis = 0; axis < 3; axis++)
+ {
+ m_pHandles[0].m_minEdges[axis] = 0;
+ m_pHandles[0].m_maxEdges[axis] = 1;
+
+ m_pEdges[axis][0].m_pos = 0;
+ m_pEdges[axis][0].m_handle = 0;
+ m_pEdges[axis][1].m_pos = m_handleSentinel;
+ m_pEdges[axis][1].m_handle = 0;
+#ifdef DEBUG_BROADPHASE
+ debugPrintAxis(axis);
+#endif //DEBUG_BROADPHASE
+
+ }
+
+}
+
+template <typename BP_FP_INT_TYPE>
+btAxisSweep3Internal<BP_FP_INT_TYPE>::~btAxisSweep3Internal()
+{
+ if (m_raycastAccelerator)
+ {
+ m_nullPairCache->~btOverlappingPairCache();
+ btAlignedFree(m_nullPairCache);
+ m_raycastAccelerator->~btDbvtBroadphase();
+ btAlignedFree (m_raycastAccelerator);
+ }
+
+ for (int i = 2; i >= 0; i--)
+ {
+ btAlignedFree(m_pEdgesRawPtr[i]);
+ }
+ delete [] m_pHandles;
+
+ if (m_ownsPairCache)
+ {
+ m_pairCache->~btOverlappingPairCache();
+ btAlignedFree(m_pairCache);
+ }
+}
+
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::quantize(BP_FP_INT_TYPE* out, const btVector3& point, int isMax) const
+{
+#ifdef OLD_CLAMPING_METHOD
+ ///problem with this clamping method is that the floating point during quantization might still go outside the range [(0|isMax) .. (m_handleSentinel&m_bpHandleMask]|isMax]
+ ///see http://code.google.com/p/bullet/issues/detail?id=87
+ btVector3 clampedPoint(point);
+ clampedPoint.setMax(m_worldAabbMin);
+ clampedPoint.setMin(m_worldAabbMax);
+ btVector3 v = (clampedPoint - m_worldAabbMin) * m_quantize;
+ out[0] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getX() & m_bpHandleMask) | isMax);
+ out[1] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getY() & m_bpHandleMask) | isMax);
+ out[2] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getZ() & m_bpHandleMask) | isMax);
+#else
+ btVector3 v = (point - m_worldAabbMin) * m_quantize;
+ out[0]=(v[0]<=0)?(BP_FP_INT_TYPE)isMax:(v[0]>=m_handleSentinel)?(BP_FP_INT_TYPE)((m_handleSentinel&m_bpHandleMask)|isMax):(BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v[0]&m_bpHandleMask)|isMax);
+ out[1]=(v[1]<=0)?(BP_FP_INT_TYPE)isMax:(v[1]>=m_handleSentinel)?(BP_FP_INT_TYPE)((m_handleSentinel&m_bpHandleMask)|isMax):(BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v[1]&m_bpHandleMask)|isMax);
+ out[2]=(v[2]<=0)?(BP_FP_INT_TYPE)isMax:(v[2]>=m_handleSentinel)?(BP_FP_INT_TYPE)((m_handleSentinel&m_bpHandleMask)|isMax):(BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v[2]&m_bpHandleMask)|isMax);
+#endif //OLD_CLAMPING_METHOD
+}
+
+
+template <typename BP_FP_INT_TYPE>
+BP_FP_INT_TYPE btAxisSweep3Internal<BP_FP_INT_TYPE>::allocHandle()
+{
+ btAssert(m_firstFreeHandle);
+
+ BP_FP_INT_TYPE handle = m_firstFreeHandle;
+ m_firstFreeHandle = getHandle(handle)->GetNextFree();
+ m_numHandles++;
+
+ return handle;
+}
+
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::freeHandle(BP_FP_INT_TYPE handle)
+{
+ btAssert(handle > 0 && handle < m_maxHandles);
+
+ getHandle(handle)->SetNextFree(m_firstFreeHandle);
+ m_firstFreeHandle = handle;
+
+ m_numHandles--;
+}
+
+
+template <typename BP_FP_INT_TYPE>
+BP_FP_INT_TYPE btAxisSweep3Internal<BP_FP_INT_TYPE>::addHandle(const btVector3& aabbMin,const btVector3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy)
+{
+ // quantize the bounds
+ BP_FP_INT_TYPE min[3], max[3];
+ quantize(min, aabbMin, 0);
+ quantize(max, aabbMax, 1);
+
+ // allocate a handle
+ BP_FP_INT_TYPE handle = allocHandle();
+
+
+ Handle* pHandle = getHandle(handle);
+
+ pHandle->m_uniqueId = static_cast<int>(handle);
+ //pHandle->m_pOverlaps = 0;
+ pHandle->m_clientObject = pOwner;
+ pHandle->m_collisionFilterGroup = collisionFilterGroup;
+ pHandle->m_collisionFilterMask = collisionFilterMask;
+ pHandle->m_multiSapParentProxy = multiSapProxy;
+
+ // compute current limit of edge arrays
+ BP_FP_INT_TYPE limit = static_cast<BP_FP_INT_TYPE>(m_numHandles * 2);
+
+
+ // insert new edges just inside the max boundary edge
+ for (BP_FP_INT_TYPE axis = 0; axis < 3; axis++)
+ {
+
+ m_pHandles[0].m_maxEdges[axis] += 2;
+
+ m_pEdges[axis][limit + 1] = m_pEdges[axis][limit - 1];
+
+ m_pEdges[axis][limit - 1].m_pos = min[axis];
+ m_pEdges[axis][limit - 1].m_handle = handle;
+
+ m_pEdges[axis][limit].m_pos = max[axis];
+ m_pEdges[axis][limit].m_handle = handle;
+
+ pHandle->m_minEdges[axis] = static_cast<BP_FP_INT_TYPE>(limit - 1);
+ pHandle->m_maxEdges[axis] = limit;
+ }
+
+ // now sort the new edges to their correct position
+ sortMinDown(0, pHandle->m_minEdges[0], dispatcher,false);
+ sortMaxDown(0, pHandle->m_maxEdges[0], dispatcher,false);
+ sortMinDown(1, pHandle->m_minEdges[1], dispatcher,false);
+ sortMaxDown(1, pHandle->m_maxEdges[1], dispatcher,false);
+ sortMinDown(2, pHandle->m_minEdges[2], dispatcher,true);
+ sortMaxDown(2, pHandle->m_maxEdges[2], dispatcher,true);
+
+
+ return handle;
+}
+
+
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::removeHandle(BP_FP_INT_TYPE handle,btDispatcher* dispatcher)
+{
+
+ Handle* pHandle = getHandle(handle);
+
+ //explicitly remove the pairs containing the proxy
+ //we could do it also in the sortMinUp (passing true)
+ ///@todo: compare performance
+ if (!m_pairCache->hasDeferredRemoval())
+ {
+ m_pairCache->removeOverlappingPairsContainingProxy(pHandle,dispatcher);
+ }
+
+ // compute current limit of edge arrays
+ int limit = static_cast<int>(m_numHandles * 2);
+
+ int axis;
+
+ for (axis = 0;axis<3;axis++)
+ {
+ m_pHandles[0].m_maxEdges[axis] -= 2;
+ }
+
+ // remove the edges by sorting them up to the end of the list
+ for ( axis = 0; axis < 3; axis++)
+ {
+ Edge* pEdges = m_pEdges[axis];
+ BP_FP_INT_TYPE max = pHandle->m_maxEdges[axis];
+ pEdges[max].m_pos = m_handleSentinel;
+
+ sortMaxUp(axis,max,dispatcher,false);
+
+
+ BP_FP_INT_TYPE i = pHandle->m_minEdges[axis];
+ pEdges[i].m_pos = m_handleSentinel;
+
+
+ sortMinUp(axis,i,dispatcher,false);
+
+ pEdges[limit-1].m_handle = 0;
+ pEdges[limit-1].m_pos = m_handleSentinel;
+
+#ifdef DEBUG_BROADPHASE
+ debugPrintAxis(axis,false);
+#endif //DEBUG_BROADPHASE
+
+
+ }
+
+
+ // free the handle
+ freeHandle(handle);
+
+
+}
+
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::resetPool(btDispatcher* dispatcher)
+{
+ if (m_numHandles == 0)
+ {
+ m_firstFreeHandle = 1;
+ {
+ for (BP_FP_INT_TYPE i = m_firstFreeHandle; i < m_maxHandles; i++)
+ m_pHandles[i].SetNextFree(static_cast<BP_FP_INT_TYPE>(i + 1));
+ m_pHandles[m_maxHandles - 1].SetNextFree(0);
+ }
+ }
+}
+
+
+extern int gOverlappingPairs;
+//#include <stdio.h>
+
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::calculateOverlappingPairs(btDispatcher* dispatcher)
+{
+
+ if (m_pairCache->hasDeferredRemoval())
+ {
+
+ btBroadphasePairArray& overlappingPairArray = m_pairCache->getOverlappingPairArray();
+
+ //perform a sort, to find duplicates and to sort 'invalid' pairs to the end
+ overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
+
+ overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
+ m_invalidPair = 0;
+
+
+ int i;
+
+ btBroadphasePair previousPair;
+ previousPair.m_pProxy0 = 0;
+ previousPair.m_pProxy1 = 0;
+ previousPair.m_algorithm = 0;
+
+
+ for (i=0;i<overlappingPairArray.size();i++)
+ {
+
+ btBroadphasePair& pair = overlappingPairArray[i];
+
+ bool isDuplicate = (pair == previousPair);
+
+ previousPair = pair;
+
+ bool needsRemoval = false;
+
+ if (!isDuplicate)
+ {
+ ///important to use an AABB test that is consistent with the broadphase
+ bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1);
+
+ if (hasOverlap)
+ {
+ needsRemoval = false;//callback->processOverlap(pair);
+ } else
+ {
+ needsRemoval = true;
+ }
+ } else
+ {
+ //remove duplicate
+ needsRemoval = true;
+ //should have no algorithm
+ btAssert(!pair.m_algorithm);
+ }
+
+ if (needsRemoval)
+ {
+ m_pairCache->cleanOverlappingPair(pair,dispatcher);
+
+ // m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
+ // m_overlappingPairArray.pop_back();
+ pair.m_pProxy0 = 0;
+ pair.m_pProxy1 = 0;
+ m_invalidPair++;
+ gOverlappingPairs--;
+ }
+
+ }
+
+ ///if you don't like to skip the invalid pairs in the array, execute following code:
+ #define CLEAN_INVALID_PAIRS 1
+ #ifdef CLEAN_INVALID_PAIRS
+
+ //perform a sort, to sort 'invalid' pairs to the end
+ overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
+
+ overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
+ m_invalidPair = 0;
+ #endif//CLEAN_INVALID_PAIRS
+
+ //printf("overlappingPairArray.size()=%d\n",overlappingPairArray.size());
+ }
+
+}
+
+
+template <typename BP_FP_INT_TYPE>
+bool btAxisSweep3Internal<BP_FP_INT_TYPE>::testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+{
+ const Handle* pHandleA = static_cast<Handle*>(proxy0);
+ const Handle* pHandleB = static_cast<Handle*>(proxy1);
+
+ //optimization 1: check the array index (memory address), instead of the m_pos
+
+ for (int axis = 0; axis < 3; axis++)
+ {
+ if (pHandleA->m_maxEdges[axis] < pHandleB->m_minEdges[axis] ||
+ pHandleB->m_maxEdges[axis] < pHandleA->m_minEdges[axis])
+ {
+ return false;
+ }
+ }
+ return true;
+}
+
+template <typename BP_FP_INT_TYPE>
+bool btAxisSweep3Internal<BP_FP_INT_TYPE>::testOverlap2D(const Handle* pHandleA, const Handle* pHandleB,int axis0,int axis1)
+{
+ //optimization 1: check the array index (memory address), instead of the m_pos
+
+ if (pHandleA->m_maxEdges[axis0] < pHandleB->m_minEdges[axis0] ||
+ pHandleB->m_maxEdges[axis0] < pHandleA->m_minEdges[axis0] ||
+ pHandleA->m_maxEdges[axis1] < pHandleB->m_minEdges[axis1] ||
+ pHandleB->m_maxEdges[axis1] < pHandleA->m_minEdges[axis1])
+ {
+ return false;
+ }
+ return true;
+}
+
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::updateHandle(BP_FP_INT_TYPE handle, const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher)
+{
+// btAssert(bounds.IsFinite());
+ //btAssert(bounds.HasVolume());
+
+ Handle* pHandle = getHandle(handle);
+
+ // quantize the new bounds
+ BP_FP_INT_TYPE min[3], max[3];
+ quantize(min, aabbMin, 0);
+ quantize(max, aabbMax, 1);
+
+ // update changed edges
+ for (int axis = 0; axis < 3; axis++)
+ {
+ BP_FP_INT_TYPE emin = pHandle->m_minEdges[axis];
+ BP_FP_INT_TYPE emax = pHandle->m_maxEdges[axis];
+
+ int dmin = (int)min[axis] - (int)m_pEdges[axis][emin].m_pos;
+ int dmax = (int)max[axis] - (int)m_pEdges[axis][emax].m_pos;
+
+ m_pEdges[axis][emin].m_pos = min[axis];
+ m_pEdges[axis][emax].m_pos = max[axis];
+
+ // expand (only adds overlaps)
+ if (dmin < 0)
+ sortMinDown(axis, emin,dispatcher,true);
+
+ if (dmax > 0)
+ sortMaxUp(axis, emax,dispatcher,true);
+
+ // shrink (only removes overlaps)
+ if (dmin > 0)
+ sortMinUp(axis, emin,dispatcher,true);
+
+ if (dmax < 0)
+ sortMaxDown(axis, emax,dispatcher,true);
+
+#ifdef DEBUG_BROADPHASE
+ debugPrintAxis(axis);
+#endif //DEBUG_BROADPHASE
+ }
+
+
+}
+
+
+
+
+// sorting a min edge downwards can only ever *add* overlaps
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMinDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* /* dispatcher */, bool updateOverlaps)
+{
+
+ Edge* pEdge = m_pEdges[axis] + edge;
+ Edge* pPrev = pEdge - 1;
+ Handle* pHandleEdge = getHandle(pEdge->m_handle);
+
+ while (pEdge->m_pos < pPrev->m_pos)
+ {
+ Handle* pHandlePrev = getHandle(pPrev->m_handle);
+
+ if (pPrev->IsMax())
+ {
+ // if previous edge is a maximum check the bounds and add an overlap if necessary
+ const int axis1 = (1 << axis) & 3;
+ const int axis2 = (1 << axis1) & 3;
+ if (updateOverlaps && testOverlap2D(pHandleEdge, pHandlePrev,axis1,axis2))
+ {
+ m_pairCache->addOverlappingPair(pHandleEdge,pHandlePrev);
+ if (m_userPairCallback)
+ m_userPairCallback->addOverlappingPair(pHandleEdge,pHandlePrev);
+
+ //AddOverlap(pEdge->m_handle, pPrev->m_handle);
+
+ }
+
+ // update edge reference in other handle
+ pHandlePrev->m_maxEdges[axis]++;
+ }
+ else
+ pHandlePrev->m_minEdges[axis]++;
+
+ pHandleEdge->m_minEdges[axis]--;
+
+ // swap the edges
+ Edge swap = *pEdge;
+ *pEdge = *pPrev;
+ *pPrev = swap;
+
+ // decrement
+ pEdge--;
+ pPrev--;
+ }
+
+#ifdef DEBUG_BROADPHASE
+ debugPrintAxis(axis);
+#endif //DEBUG_BROADPHASE
+
+}
+
+// sorting a min edge upwards can only ever *remove* overlaps
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMinUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps)
+{
+ Edge* pEdge = m_pEdges[axis] + edge;
+ Edge* pNext = pEdge + 1;
+ Handle* pHandleEdge = getHandle(pEdge->m_handle);
+
+ while (pNext->m_handle && (pEdge->m_pos >= pNext->m_pos))
+ {
+ Handle* pHandleNext = getHandle(pNext->m_handle);
+
+ if (pNext->IsMax())
+ {
+ Handle* handle0 = getHandle(pEdge->m_handle);
+ Handle* handle1 = getHandle(pNext->m_handle);
+ const int axis1 = (1 << axis) & 3;
+ const int axis2 = (1 << axis1) & 3;
+
+ // if next edge is maximum remove any overlap between the two handles
+ if (updateOverlaps
+#ifdef USE_OVERLAP_TEST_ON_REMOVES
+ && testOverlap2D(handle0,handle1,axis1,axis2)
+#endif //USE_OVERLAP_TEST_ON_REMOVES
+ )
+ {
+
+
+ m_pairCache->removeOverlappingPair(handle0,handle1,dispatcher);
+ if (m_userPairCallback)
+ m_userPairCallback->removeOverlappingPair(handle0,handle1,dispatcher);
+
+ }
+
+
+ // update edge reference in other handle
+ pHandleNext->m_maxEdges[axis]--;
+ }
+ else
+ pHandleNext->m_minEdges[axis]--;
+
+ pHandleEdge->m_minEdges[axis]++;
+
+ // swap the edges
+ Edge swap = *pEdge;
+ *pEdge = *pNext;
+ *pNext = swap;
+
+ // increment
+ pEdge++;
+ pNext++;
+ }
+
+
+}
+
+// sorting a max edge downwards can only ever *remove* overlaps
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMaxDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps)
+{
+
+ Edge* pEdge = m_pEdges[axis] + edge;
+ Edge* pPrev = pEdge - 1;
+ Handle* pHandleEdge = getHandle(pEdge->m_handle);
+
+ while (pEdge->m_pos < pPrev->m_pos)
+ {
+ Handle* pHandlePrev = getHandle(pPrev->m_handle);
+
+ if (!pPrev->IsMax())
+ {
+ // if previous edge was a minimum remove any overlap between the two handles
+ Handle* handle0 = getHandle(pEdge->m_handle);
+ Handle* handle1 = getHandle(pPrev->m_handle);
+ const int axis1 = (1 << axis) & 3;
+ const int axis2 = (1 << axis1) & 3;
+
+ if (updateOverlaps
+#ifdef USE_OVERLAP_TEST_ON_REMOVES
+ && testOverlap2D(handle0,handle1,axis1,axis2)
+#endif //USE_OVERLAP_TEST_ON_REMOVES
+ )
+ {
+ //this is done during the overlappingpairarray iteration/narrowphase collision
+
+
+ m_pairCache->removeOverlappingPair(handle0,handle1,dispatcher);
+ if (m_userPairCallback)
+ m_userPairCallback->removeOverlappingPair(handle0,handle1,dispatcher);
+
+
+
+ }
+
+ // update edge reference in other handle
+ pHandlePrev->m_minEdges[axis]++;;
+ }
+ else
+ pHandlePrev->m_maxEdges[axis]++;
+
+ pHandleEdge->m_maxEdges[axis]--;
+
+ // swap the edges
+ Edge swap = *pEdge;
+ *pEdge = *pPrev;
+ *pPrev = swap;
+
+ // decrement
+ pEdge--;
+ pPrev--;
+ }
+
+
+#ifdef DEBUG_BROADPHASE
+ debugPrintAxis(axis);
+#endif //DEBUG_BROADPHASE
+
+}
+
+// sorting a max edge upwards can only ever *add* overlaps
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMaxUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* /* dispatcher */, bool updateOverlaps)
+{
+ Edge* pEdge = m_pEdges[axis] + edge;
+ Edge* pNext = pEdge + 1;
+ Handle* pHandleEdge = getHandle(pEdge->m_handle);
+
+ while (pNext->m_handle && (pEdge->m_pos >= pNext->m_pos))
+ {
+ Handle* pHandleNext = getHandle(pNext->m_handle);
+
+ const int axis1 = (1 << axis) & 3;
+ const int axis2 = (1 << axis1) & 3;
+
+ if (!pNext->IsMax())
+ {
+ // if next edge is a minimum check the bounds and add an overlap if necessary
+ if (updateOverlaps && testOverlap2D(pHandleEdge, pHandleNext,axis1,axis2))
+ {
+ Handle* handle0 = getHandle(pEdge->m_handle);
+ Handle* handle1 = getHandle(pNext->m_handle);
+ m_pairCache->addOverlappingPair(handle0,handle1);
+ if (m_userPairCallback)
+ m_userPairCallback->addOverlappingPair(handle0,handle1);
+ }
+
+ // update edge reference in other handle
+ pHandleNext->m_minEdges[axis]--;
+ }
+ else
+ pHandleNext->m_maxEdges[axis]--;
+
+ pHandleEdge->m_maxEdges[axis]++;
+
+ // swap the edges
+ Edge swap = *pEdge;
+ *pEdge = *pNext;
+ *pNext = swap;
+
+ // increment
+ pEdge++;
+ pNext++;
+ }
+
+}
+
+
+
+////////////////////////////////////////////////////////////////////
+
+
+/// The btAxisSweep3 is an efficient implementation of the 3d axis sweep and prune broadphase.
+/// It uses arrays rather then lists for storage of the 3 axis. Also it operates using 16 bit integer coordinates instead of floats.
+/// For large worlds and many objects, use bt32BitAxisSweep3 or btDbvtBroadphase instead. bt32BitAxisSweep3 has higher precision and allows more then 16384 objects at the cost of more memory and bit of performance.
+class btAxisSweep3 : public btAxisSweep3Internal<unsigned short int>
+{
+public:
+
+ btAxisSweep3(const btVector3& worldAabbMin,const btVector3& worldAabbMax, unsigned short int maxHandles = 16384, btOverlappingPairCache* pairCache = 0, bool disableRaycastAccelerator = false);
+
+};
+
+/// The bt32BitAxisSweep3 allows higher precision quantization and more objects compared to the btAxisSweep3 sweep and prune.
+/// This comes at the cost of more memory per handle, and a bit slower performance.
+/// It uses arrays rather then lists for storage of the 3 axis.
+class bt32BitAxisSweep3 : public btAxisSweep3Internal<unsigned int>
+{
+public:
+
+ bt32BitAxisSweep3(const btVector3& worldAabbMin,const btVector3& worldAabbMax, unsigned int maxHandles = 1500000, btOverlappingPairCache* pairCache = 0, bool disableRaycastAccelerator = false);
+
+};
+
+#endif
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,74 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BROADPHASE_INTERFACE_H
+#define BROADPHASE_INTERFACE_H
+
+
+
+struct btDispatcherInfo;
+class btDispatcher;
+#include "btBroadphaseProxy.h"
+
+class btOverlappingPairCache;
+
+
+
+struct btBroadphaseRayCallback
+{
+ ///added some cached data to accelerate ray-AABB tests
+ btVector3 m_rayDirectionInverse;
+ unsigned int m_signs[3];
+ btScalar m_lambda_max;
+
+ virtual ~btBroadphaseRayCallback() {}
+ virtual bool process(const btBroadphaseProxy* proxy) = 0;
+};
+
+#include "LinearMath/btVector3.h"
+
+///The btBroadphaseInterface class provides an interface to detect aabb-overlapping object pairs.
+///Some implementations for this broadphase interface include btAxisSweep3, bt32BitAxisSweep3 and btDbvtBroadphase.
+///The actual overlapping pair management, storage, adding and removing of pairs is dealt by the btOverlappingPairCache class.
+class btBroadphaseInterface
+{
+public:
+ virtual ~btBroadphaseInterface() {}
+
+ virtual btBroadphaseProxy* createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* multiSapProxy) =0;
+ virtual void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher)=0;
+ virtual void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher)=0;
+ virtual void getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const =0;
+
+ virtual void rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin=btVector3(0,0,0), const btVector3& aabbMax = btVector3(0,0,0)) = 0;
+
+ ///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb
+ virtual void calculateOverlappingPairs(btDispatcher* dispatcher)=0;
+
+ virtual btOverlappingPairCache* getOverlappingPairCache()=0;
+ virtual const btOverlappingPairCache* getOverlappingPairCache() const =0;
+
+ ///getAabb returns the axis aligned bounding box in the 'global' coordinate frame
+ ///will add some transform later
+ virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const =0;
+
+ ///reset broadphase internal structures, to ensure determinism/reproducability
+ virtual void resetPool(btDispatcher* dispatcher) {};
+
+ virtual void printStats() = 0;
+
+};
+
+#endif //BROADPHASE_INTERFACE_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,259 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BROADPHASE_PROXY_H
+#define BROADPHASE_PROXY_H
+
+#include "LinearMath/btScalar.h" //for SIMD_FORCE_INLINE
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btAlignedAllocator.h"
+
+
+/// btDispatcher uses these types
+/// IMPORTANT NOTE:The types are ordered polyhedral, implicit convex and concave
+/// to facilitate type checking
+/// CUSTOM_POLYHEDRAL_SHAPE_TYPE,CUSTOM_CONVEX_SHAPE_TYPE and CUSTOM_CONCAVE_SHAPE_TYPE can be used to extend Bullet without modifying source code
+enum BroadphaseNativeTypes
+{
+ // polyhedral convex shapes
+ BOX_SHAPE_PROXYTYPE,
+ TRIANGLE_SHAPE_PROXYTYPE,
+ TETRAHEDRAL_SHAPE_PROXYTYPE,
+ CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE,
+ CONVEX_HULL_SHAPE_PROXYTYPE,
+ CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE,
+ CUSTOM_POLYHEDRAL_SHAPE_TYPE,
+//implicit convex shapes
+IMPLICIT_CONVEX_SHAPES_START_HERE,
+ SPHERE_SHAPE_PROXYTYPE,
+ MULTI_SPHERE_SHAPE_PROXYTYPE,
+ CAPSULE_SHAPE_PROXYTYPE,
+ CONE_SHAPE_PROXYTYPE,
+ CONVEX_SHAPE_PROXYTYPE,
+ CYLINDER_SHAPE_PROXYTYPE,
+ UNIFORM_SCALING_SHAPE_PROXYTYPE,
+ MINKOWSKI_SUM_SHAPE_PROXYTYPE,
+ MINKOWSKI_DIFFERENCE_SHAPE_PROXYTYPE,
+ BOX_2D_SHAPE_PROXYTYPE,
+ CONVEX_2D_SHAPE_PROXYTYPE,
+ CUSTOM_CONVEX_SHAPE_TYPE,
+//concave shapes
+CONCAVE_SHAPES_START_HERE,
+ //keep all the convex shapetype below here, for the check IsConvexShape in broadphase proxy!
+ TRIANGLE_MESH_SHAPE_PROXYTYPE,
+ SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE,
+ ///used for demo integration FAST/Swift collision library and Bullet
+ FAST_CONCAVE_MESH_PROXYTYPE,
+ //terrain
+ TERRAIN_SHAPE_PROXYTYPE,
+///Used for GIMPACT Trimesh integration
+ GIMPACT_SHAPE_PROXYTYPE,
+///Multimaterial mesh
+ MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE,
+
+ EMPTY_SHAPE_PROXYTYPE,
+ STATIC_PLANE_PROXYTYPE,
+ CUSTOM_CONCAVE_SHAPE_TYPE,
+CONCAVE_SHAPES_END_HERE,
+
+ COMPOUND_SHAPE_PROXYTYPE,
+
+ SOFTBODY_SHAPE_PROXYTYPE,
+ HFFLUID_SHAPE_PROXYTYPE,
+ HFFLUID_BUOYANT_CONVEX_SHAPE_PROXYTYPE,
+ INVALID_SHAPE_PROXYTYPE,
+
+ MAX_BROADPHASE_COLLISION_TYPES
+
+};
+
+
+///The btBroadphaseProxy is the main class that can be used with the Bullet broadphases.
+///It stores collision shape type information, collision filter information and a client object, typically a btCollisionObject or btRigidBody.
+ATTRIBUTE_ALIGNED16(struct) btBroadphaseProxy
+{
+
+BT_DECLARE_ALIGNED_ALLOCATOR();
+
+ ///optional filtering to cull potential collisions
+ enum CollisionFilterGroups
+ {
+ DefaultFilter = 1,
+ StaticFilter = 2,
+ KinematicFilter = 4,
+ DebrisFilter = 8,
+ SensorTrigger = 16,
+ CharacterFilter = 32,
+ AllFilter = -1 //all bits sets: DefaultFilter | StaticFilter | KinematicFilter | DebrisFilter | SensorTrigger
+ };
+
+ //Usually the client btCollisionObject or Rigidbody class
+ void* m_clientObject;
+ short int m_collisionFilterGroup;
+ short int m_collisionFilterMask;
+ void* m_multiSapParentProxy;
+ int m_uniqueId;//m_uniqueId is introduced for paircache. could get rid of this, by calculating the address offset etc.
+
+ btVector3 m_aabbMin;
+ btVector3 m_aabbMax;
+
+ SIMD_FORCE_INLINE int getUid() const
+ {
+ return m_uniqueId;
+ }
+
+ //used for memory pools
+ btBroadphaseProxy() :m_clientObject(0),m_multiSapParentProxy(0)
+ {
+ }
+
+ btBroadphaseProxy(const btVector3& aabbMin,const btVector3& aabbMax,void* userPtr,short int collisionFilterGroup, short int collisionFilterMask,void* multiSapParentProxy=0)
+ :m_clientObject(userPtr),
+ m_collisionFilterGroup(collisionFilterGroup),
+ m_collisionFilterMask(collisionFilterMask),
+ m_aabbMin(aabbMin),
+ m_aabbMax(aabbMax)
+ {
+ m_multiSapParentProxy = multiSapParentProxy;
+ }
+
+
+
+ static SIMD_FORCE_INLINE bool isPolyhedral(int proxyType)
+ {
+ return (proxyType < IMPLICIT_CONVEX_SHAPES_START_HERE);
+ }
+
+ static SIMD_FORCE_INLINE bool isConvex(int proxyType)
+ {
+ return (proxyType < CONCAVE_SHAPES_START_HERE);
+ }
+
+ static SIMD_FORCE_INLINE bool isConcave(int proxyType)
+ {
+ return ((proxyType > CONCAVE_SHAPES_START_HERE) &&
+ (proxyType < CONCAVE_SHAPES_END_HERE));
+ }
+ static SIMD_FORCE_INLINE bool isCompound(int proxyType)
+ {
+ return (proxyType == COMPOUND_SHAPE_PROXYTYPE);
+ }
+ static SIMD_FORCE_INLINE bool isInfinite(int proxyType)
+ {
+ return (proxyType == STATIC_PLANE_PROXYTYPE);
+ }
+
+ static SIMD_FORCE_INLINE bool isConvex2d(int proxyType)
+ {
+ return (proxyType == BOX_2D_SHAPE_PROXYTYPE) || (proxyType == CONVEX_2D_SHAPE_PROXYTYPE);
+ }
+
+
+}
+;
+
+class btCollisionAlgorithm;
+
+struct btBroadphaseProxy;
+
+
+
+///The btBroadphasePair class contains a pair of aabb-overlapping objects.
+///A btDispatcher can search a btCollisionAlgorithm that performs exact/narrowphase collision detection on the actual collision shapes.
+ATTRIBUTE_ALIGNED16(struct) btBroadphasePair
+{
+ btBroadphasePair ()
+ :
+ m_pProxy0(0),
+ m_pProxy1(0),
+ m_algorithm(0),
+ m_internalInfo1(0)
+ {
+ }
+
+BT_DECLARE_ALIGNED_ALLOCATOR();
+
+ btBroadphasePair(const btBroadphasePair& other)
+ : m_pProxy0(other.m_pProxy0),
+ m_pProxy1(other.m_pProxy1),
+ m_algorithm(other.m_algorithm),
+ m_internalInfo1(other.m_internalInfo1)
+ {
+ }
+ btBroadphasePair(btBroadphaseProxy& proxy0,btBroadphaseProxy& proxy1)
+ {
+
+ //keep them sorted, so the std::set operations work
+ if (proxy0.m_uniqueId < proxy1.m_uniqueId)
+ {
+ m_pProxy0 = &proxy0;
+ m_pProxy1 = &proxy1;
+ }
+ else
+ {
+ m_pProxy0 = &proxy1;
+ m_pProxy1 = &proxy0;
+ }
+
+ m_algorithm = 0;
+ m_internalInfo1 = 0;
+
+ }
+
+ btBroadphaseProxy* m_pProxy0;
+ btBroadphaseProxy* m_pProxy1;
+
+ mutable btCollisionAlgorithm* m_algorithm;
+ union { void* m_internalInfo1; int m_internalTmpValue;};//don't use this data, it will be removed in future version.
+
+};
+
+/*
+//comparison for set operation, see Solid DT_Encounter
+SIMD_FORCE_INLINE bool operator<(const btBroadphasePair& a, const btBroadphasePair& b)
+{
+ return a.m_pProxy0 < b.m_pProxy0 ||
+ (a.m_pProxy0 == b.m_pProxy0 && a.m_pProxy1 < b.m_pProxy1);
+}
+*/
+
+
+
+class btBroadphasePairSortPredicate
+{
+ public:
+
+ bool operator() ( const btBroadphasePair& a, const btBroadphasePair& b )
+ {
+ const int uidA0 = a.m_pProxy0 ? a.m_pProxy0->m_uniqueId : -1;
+ const int uidB0 = b.m_pProxy0 ? b.m_pProxy0->m_uniqueId : -1;
+ const int uidA1 = a.m_pProxy1 ? a.m_pProxy1->m_uniqueId : -1;
+ const int uidB1 = b.m_pProxy1 ? b.m_pProxy1->m_uniqueId : -1;
+
+ return uidA0 > uidB0 ||
+ (a.m_pProxy0 == b.m_pProxy0 && uidA1 > uidB1) ||
+ (a.m_pProxy0 == b.m_pProxy0 && a.m_pProxy1 == b.m_pProxy1 && a.m_algorithm > b.m_algorithm);
+ }
+};
+
+
+SIMD_FORCE_INLINE bool operator==(const btBroadphasePair& a, const btBroadphasePair& b)
+{
+ return (a.m_pProxy0 == b.m_pProxy0) && (a.m_pProxy1 == b.m_pProxy1);
+}
+
+
+#endif //BROADPHASE_PROXY_H
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,80 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef COLLISION_ALGORITHM_H
+#define COLLISION_ALGORITHM_H
+
+#include "LinearMath/btScalar.h"
+#include "LinearMath/btAlignedObjectArray.h"
+
+struct btBroadphaseProxy;
+class btDispatcher;
+class btManifoldResult;
+class btCollisionObject;
+struct btDispatcherInfo;
+class btPersistentManifold;
+
+typedef btAlignedObjectArray<btPersistentManifold*> btManifoldArray;
+
+struct btCollisionAlgorithmConstructionInfo
+{
+ btCollisionAlgorithmConstructionInfo()
+ :m_dispatcher1(0),
+ m_manifold(0)
+ {
+ }
+ btCollisionAlgorithmConstructionInfo(btDispatcher* dispatcher,int temp)
+ :m_dispatcher1(dispatcher)
+ {
+ (void)temp;
+ }
+
+ btDispatcher* m_dispatcher1;
+ btPersistentManifold* m_manifold;
+
+ int getDispatcherId();
+
+};
+
+
+///btCollisionAlgorithm is an collision interface that is compatible with the Broadphase and btDispatcher.
+///It is persistent over frames
+class btCollisionAlgorithm
+{
+
+protected:
+
+ btDispatcher* m_dispatcher;
+
+protected:
+ int getDispatcherId();
+
+public:
+
+ btCollisionAlgorithm() {};
+
+ btCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci);
+
+ virtual ~btCollisionAlgorithm() {};
+
+ virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) = 0;
+
+ virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) = 0;
+
+ virtual void getAllContactManifolds(btManifoldArray& manifoldArray) = 0;
+};
+
+
+#endif //COLLISION_ALGORITHM_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btDbvt.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btDbvt.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btDbvt.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btDbvt.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,1255 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+///btDbvt implementation by Nathanael Presson
+
+#ifndef BT_DYNAMIC_BOUNDING_VOLUME_TREE_H
+#define BT_DYNAMIC_BOUNDING_VOLUME_TREE_H
+
+#include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btAabbUtil2.h"
+
+//
+// Compile time configuration
+//
+
+
+// Implementation profiles
+#define DBVT_IMPL_GENERIC 0 // Generic implementation
+#define DBVT_IMPL_SSE 1 // SSE
+
+// Template implementation of ICollide
+#ifdef WIN32
+#if (defined (_MSC_VER) && _MSC_VER >= 1400)
+#define DBVT_USE_TEMPLATE 1
+#else
+#define DBVT_USE_TEMPLATE 0
+#endif
+#else
+#define DBVT_USE_TEMPLATE 0
+#endif
+
+// Use only intrinsics instead of inline asm
+#define DBVT_USE_INTRINSIC_SSE 1
+
+// Using memmov for collideOCL
+#define DBVT_USE_MEMMOVE 1
+
+// Enable benchmarking code
+#define DBVT_ENABLE_BENCHMARK 0
+
+// Inlining
+#define DBVT_INLINE SIMD_FORCE_INLINE
+
+// Specific methods implementation
+
+//SSE gives errors on a MSVC 7.1
+#if defined (BT_USE_SSE) && defined (WIN32)
+#define DBVT_SELECT_IMPL DBVT_IMPL_SSE
+#define DBVT_MERGE_IMPL DBVT_IMPL_SSE
+#define DBVT_INT0_IMPL DBVT_IMPL_SSE
+#else
+#define DBVT_SELECT_IMPL DBVT_IMPL_GENERIC
+#define DBVT_MERGE_IMPL DBVT_IMPL_GENERIC
+#define DBVT_INT0_IMPL DBVT_IMPL_GENERIC
+#endif
+
+#if (DBVT_SELECT_IMPL==DBVT_IMPL_SSE)|| \
+ (DBVT_MERGE_IMPL==DBVT_IMPL_SSE)|| \
+ (DBVT_INT0_IMPL==DBVT_IMPL_SSE)
+#include <emmintrin.h>
+#endif
+
+//
+// Auto config and checks
+//
+
+#if DBVT_USE_TEMPLATE
+#define DBVT_VIRTUAL
+#define DBVT_VIRTUAL_DTOR(a)
+#define DBVT_PREFIX template <typename T>
+#define DBVT_IPOLICY T& policy
+#define DBVT_CHECKTYPE static const ICollide& typechecker=*(T*)1;(void)typechecker;
+#else
+#define DBVT_VIRTUAL_DTOR(a) virtual ~a() {}
+#define DBVT_VIRTUAL virtual
+#define DBVT_PREFIX
+#define DBVT_IPOLICY ICollide& policy
+#define DBVT_CHECKTYPE
+#endif
+
+#if DBVT_USE_MEMMOVE
+#ifndef __CELLOS_LV2__
+#include <memory.h>
+#endif
+#include <string.h>
+#endif
+
+#ifndef DBVT_USE_TEMPLATE
+#error "DBVT_USE_TEMPLATE undefined"
+#endif
+
+#ifndef DBVT_USE_MEMMOVE
+#error "DBVT_USE_MEMMOVE undefined"
+#endif
+
+#ifndef DBVT_ENABLE_BENCHMARK
+#error "DBVT_ENABLE_BENCHMARK undefined"
+#endif
+
+#ifndef DBVT_SELECT_IMPL
+#error "DBVT_SELECT_IMPL undefined"
+#endif
+
+#ifndef DBVT_MERGE_IMPL
+#error "DBVT_MERGE_IMPL undefined"
+#endif
+
+#ifndef DBVT_INT0_IMPL
+#error "DBVT_INT0_IMPL undefined"
+#endif
+
+//
+// Defaults volumes
+//
+
+/* btDbvtAabbMm */
+struct btDbvtAabbMm
+{
+ DBVT_INLINE btVector3 Center() const { return((mi+mx)/2); }
+ DBVT_INLINE btVector3 Lengths() const { return(mx-mi); }
+ DBVT_INLINE btVector3 Extents() const { return((mx-mi)/2); }
+ DBVT_INLINE const btVector3& Mins() const { return(mi); }
+ DBVT_INLINE const btVector3& Maxs() const { return(mx); }
+ static inline btDbvtAabbMm FromCE(const btVector3& c,const btVector3& e);
+ static inline btDbvtAabbMm FromCR(const btVector3& c,btScalar r);
+ static inline btDbvtAabbMm FromMM(const btVector3& mi,const btVector3& mx);
+ static inline btDbvtAabbMm FromPoints(const btVector3* pts,int n);
+ static inline btDbvtAabbMm FromPoints(const btVector3** ppts,int n);
+ DBVT_INLINE void Expand(const btVector3& e);
+ DBVT_INLINE void SignedExpand(const btVector3& e);
+ DBVT_INLINE bool Contain(const btDbvtAabbMm& a) const;
+ DBVT_INLINE int Classify(const btVector3& n,btScalar o,int s) const;
+ DBVT_INLINE btScalar ProjectMinimum(const btVector3& v,unsigned signs) const;
+ DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a,
+ const btDbvtAabbMm& b);
+
+ DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a,
+ const btVector3& b);
+
+ DBVT_INLINE friend btScalar Proximity( const btDbvtAabbMm& a,
+ const btDbvtAabbMm& b);
+ DBVT_INLINE friend int Select( const btDbvtAabbMm& o,
+ const btDbvtAabbMm& a,
+ const btDbvtAabbMm& b);
+ DBVT_INLINE friend void Merge( const btDbvtAabbMm& a,
+ const btDbvtAabbMm& b,
+ btDbvtAabbMm& r);
+ DBVT_INLINE friend bool NotEqual( const btDbvtAabbMm& a,
+ const btDbvtAabbMm& b);
+private:
+ DBVT_INLINE void AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const;
+private:
+ btVector3 mi,mx;
+};
+
+// Types
+typedef btDbvtAabbMm btDbvtVolume;
+
+/* btDbvtNode */
+struct btDbvtNode
+{
+ btDbvtVolume volume;
+ btDbvtNode* parent;
+ DBVT_INLINE bool isleaf() const { return(childs[1]==0); }
+ DBVT_INLINE bool isinternal() const { return(!isleaf()); }
+ union
+ {
+ btDbvtNode* childs[2];
+ void* data;
+ int dataAsInt;
+ };
+};
+
+///The btDbvt class implements a fast dynamic bounding volume tree based on axis aligned bounding boxes (aabb tree).
+///This btDbvt is used for soft body collision detection and for the btDbvtBroadphase. It has a fast insert, remove and update of nodes.
+///Unlike the btQuantizedBvh, nodes can be dynamically moved around, which allows for change in topology of the underlying data structure.
+struct btDbvt
+{
+ /* Stack element */
+ struct sStkNN
+ {
+ const btDbvtNode* a;
+ const btDbvtNode* b;
+ sStkNN() {}
+ sStkNN(const btDbvtNode* na,const btDbvtNode* nb) : a(na),b(nb) {}
+ };
+ struct sStkNP
+ {
+ const btDbvtNode* node;
+ int mask;
+ sStkNP(const btDbvtNode* n,unsigned m) : node(n),mask(m) {}
+ };
+ struct sStkNPS
+ {
+ const btDbvtNode* node;
+ int mask;
+ btScalar value;
+ sStkNPS() {}
+ sStkNPS(const btDbvtNode* n,unsigned m,btScalar v) : node(n),mask(m),value(v) {}
+ };
+ struct sStkCLN
+ {
+ const btDbvtNode* node;
+ btDbvtNode* parent;
+ sStkCLN(const btDbvtNode* n,btDbvtNode* p) : node(n),parent(p) {}
+ };
+ // Policies/Interfaces
+
+ /* ICollide */
+ struct ICollide
+ {
+ DBVT_VIRTUAL_DTOR(ICollide)
+ DBVT_VIRTUAL void Process(const btDbvtNode*,const btDbvtNode*) {}
+ DBVT_VIRTUAL void Process(const btDbvtNode*) {}
+ DBVT_VIRTUAL void Process(const btDbvtNode* n,btScalar) { Process(n); }
+ DBVT_VIRTUAL bool Descent(const btDbvtNode*) { return(true); }
+ DBVT_VIRTUAL bool AllLeaves(const btDbvtNode*) { return(true); }
+ };
+ /* IWriter */
+ struct IWriter
+ {
+ virtual ~IWriter() {}
+ virtual void Prepare(const btDbvtNode* root,int numnodes)=0;
+ virtual void WriteNode(const btDbvtNode*,int index,int parent,int child0,int child1)=0;
+ virtual void WriteLeaf(const btDbvtNode*,int index,int parent)=0;
+ };
+ /* IClone */
+ struct IClone
+ {
+ virtual ~IClone() {}
+ virtual void CloneLeaf(btDbvtNode*) {}
+ };
+
+ // Constants
+ enum {
+ SIMPLE_STACKSIZE = 64,
+ DOUBLE_STACKSIZE = SIMPLE_STACKSIZE*2
+ };
+
+ // Fields
+ btDbvtNode* m_root;
+ btDbvtNode* m_free;
+ int m_lkhd;
+ int m_leaves;
+ unsigned m_opath;
+
+
+ btAlignedObjectArray<sStkNN> m_stkStack;
+
+
+ // Methods
+ btDbvt();
+ ~btDbvt();
+ void clear();
+ bool empty() const { return(0==m_root); }
+ void optimizeBottomUp();
+ void optimizeTopDown(int bu_treshold=128);
+ void optimizeIncremental(int passes);
+ btDbvtNode* insert(const btDbvtVolume& box,void* data);
+ void update(btDbvtNode* leaf,int lookahead=-1);
+ void update(btDbvtNode* leaf,btDbvtVolume& volume);
+ bool update(btDbvtNode* leaf,btDbvtVolume& volume,const btVector3& velocity,btScalar margin);
+ bool update(btDbvtNode* leaf,btDbvtVolume& volume,const btVector3& velocity);
+ bool update(btDbvtNode* leaf,btDbvtVolume& volume,btScalar margin);
+ void remove(btDbvtNode* leaf);
+ void write(IWriter* iwriter) const;
+ void clone(btDbvt& dest,IClone* iclone=0) const;
+ static int maxdepth(const btDbvtNode* node);
+ static int countLeaves(const btDbvtNode* node);
+ static void extractLeaves(const btDbvtNode* node,btAlignedObjectArray<const btDbvtNode*>& leaves);
+#if DBVT_ENABLE_BENCHMARK
+ static void benchmark();
+#else
+ static void benchmark(){}
+#endif
+ // DBVT_IPOLICY must support ICollide policy/interface
+ DBVT_PREFIX
+ static void enumNodes( const btDbvtNode* root,
+ DBVT_IPOLICY);
+ DBVT_PREFIX
+ static void enumLeaves( const btDbvtNode* root,
+ DBVT_IPOLICY);
+ DBVT_PREFIX
+ void collideTT( const btDbvtNode* root0,
+ const btDbvtNode* root1,
+ DBVT_IPOLICY);
+
+ DBVT_PREFIX
+ void collideTTpersistentStack( const btDbvtNode* root0,
+ const btDbvtNode* root1,
+ DBVT_IPOLICY);
+#if 0
+ DBVT_PREFIX
+ void collideTT( const btDbvtNode* root0,
+ const btDbvtNode* root1,
+ const btTransform& xform,
+ DBVT_IPOLICY);
+ DBVT_PREFIX
+ void collideTT( const btDbvtNode* root0,
+ const btTransform& xform0,
+ const btDbvtNode* root1,
+ const btTransform& xform1,
+ DBVT_IPOLICY);
+#endif
+
+ DBVT_PREFIX
+ void collideTV( const btDbvtNode* root,
+ const btDbvtVolume& volume,
+ DBVT_IPOLICY);
+ ///rayTest is a re-entrant ray test, and can be called in parallel as long as the btAlignedAlloc is thread-safe (uses locking etc)
+ ///rayTest is slower than rayTestInternal, because it builds a local stack, using memory allocations, and it recomputes signs/rayDirectionInverses each time
+ DBVT_PREFIX
+ static void rayTest( const btDbvtNode* root,
+ const btVector3& rayFrom,
+ const btVector3& rayTo,
+ DBVT_IPOLICY);
+ ///rayTestInternal is faster than rayTest, because it uses a persistent stack (to reduce dynamic memory allocations to a minimum) and it uses precomputed signs/rayInverseDirections
+ ///rayTestInternal is used by btDbvtBroadphase to accelerate world ray casts
+ DBVT_PREFIX
+ void rayTestInternal( const btDbvtNode* root,
+ const btVector3& rayFrom,
+ const btVector3& rayTo,
+ const btVector3& rayDirectionInverse,
+ unsigned int signs[3],
+ btScalar lambda_max,
+ const btVector3& aabbMin,
+ const btVector3& aabbMax,
+ DBVT_IPOLICY) const;
+
+ DBVT_PREFIX
+ static void collideKDOP(const btDbvtNode* root,
+ const btVector3* normals,
+ const btScalar* offsets,
+ int count,
+ DBVT_IPOLICY);
+ DBVT_PREFIX
+ static void collideOCL( const btDbvtNode* root,
+ const btVector3* normals,
+ const btScalar* offsets,
+ const btVector3& sortaxis,
+ int count,
+ DBVT_IPOLICY,
+ bool fullsort=true);
+ DBVT_PREFIX
+ static void collideTU( const btDbvtNode* root,
+ DBVT_IPOLICY);
+ // Helpers
+ static DBVT_INLINE int nearest(const int* i,const btDbvt::sStkNPS* a,btScalar v,int l,int h)
+ {
+ int m=0;
+ while(l<h)
+ {
+ m=(l+h)>>1;
+ if(a[i[m]].value>=v) l=m+1; else h=m;
+ }
+ return(h);
+ }
+ static DBVT_INLINE int allocate( btAlignedObjectArray<int>& ifree,
+ btAlignedObjectArray<sStkNPS>& stock,
+ const sStkNPS& value)
+ {
+ int i;
+ if(ifree.size()>0)
+ { i=ifree[ifree.size()-1];ifree.pop_back();stock[i]=value; }
+ else
+ { i=stock.size();stock.push_back(value); }
+ return(i);
+ }
+ //
+private:
+ btDbvt(const btDbvt&) {}
+};
+
+//
+// Inline's
+//
+
+//
+inline btDbvtAabbMm btDbvtAabbMm::FromCE(const btVector3& c,const btVector3& e)
+{
+ btDbvtAabbMm box;
+ box.mi=c-e;box.mx=c+e;
+ return(box);
+}
+
+//
+inline btDbvtAabbMm btDbvtAabbMm::FromCR(const btVector3& c,btScalar r)
+{
+ return(FromCE(c,btVector3(r,r,r)));
+}
+
+//
+inline btDbvtAabbMm btDbvtAabbMm::FromMM(const btVector3& mi,const btVector3& mx)
+{
+ btDbvtAabbMm box;
+ box.mi=mi;box.mx=mx;
+ return(box);
+}
+
+//
+inline btDbvtAabbMm btDbvtAabbMm::FromPoints(const btVector3* pts,int n)
+{
+ btDbvtAabbMm box;
+ box.mi=box.mx=pts[0];
+ for(int i=1;i<n;++i)
+ {
+ box.mi.setMin(pts[i]);
+ box.mx.setMax(pts[i]);
+ }
+ return(box);
+}
+
+//
+inline btDbvtAabbMm btDbvtAabbMm::FromPoints(const btVector3** ppts,int n)
+{
+ btDbvtAabbMm box;
+ box.mi=box.mx=*ppts[0];
+ for(int i=1;i<n;++i)
+ {
+ box.mi.setMin(*ppts[i]);
+ box.mx.setMax(*ppts[i]);
+ }
+ return(box);
+}
+
+//
+DBVT_INLINE void btDbvtAabbMm::Expand(const btVector3& e)
+{
+ mi-=e;mx+=e;
+}
+
+//
+DBVT_INLINE void btDbvtAabbMm::SignedExpand(const btVector3& e)
+{
+ if(e.x()>0) mx.setX(mx.x()+e[0]); else mi.setX(mi.x()+e[0]);
+ if(e.y()>0) mx.setY(mx.y()+e[1]); else mi.setY(mi.y()+e[1]);
+ if(e.z()>0) mx.setZ(mx.z()+e[2]); else mi.setZ(mi.z()+e[2]);
+}
+
+//
+DBVT_INLINE bool btDbvtAabbMm::Contain(const btDbvtAabbMm& a) const
+{
+ return( (mi.x()<=a.mi.x())&&
+ (mi.y()<=a.mi.y())&&
+ (mi.z()<=a.mi.z())&&
+ (mx.x()>=a.mx.x())&&
+ (mx.y()>=a.mx.y())&&
+ (mx.z()>=a.mx.z()));
+}
+
+//
+DBVT_INLINE int btDbvtAabbMm::Classify(const btVector3& n,btScalar o,int s) const
+{
+ btVector3 pi,px;
+ switch(s)
+ {
+ case (0+0+0): px=btVector3(mi.x(),mi.y(),mi.z());
+ pi=btVector3(mx.x(),mx.y(),mx.z());break;
+ case (1+0+0): px=btVector3(mx.x(),mi.y(),mi.z());
+ pi=btVector3(mi.x(),mx.y(),mx.z());break;
+ case (0+2+0): px=btVector3(mi.x(),mx.y(),mi.z());
+ pi=btVector3(mx.x(),mi.y(),mx.z());break;
+ case (1+2+0): px=btVector3(mx.x(),mx.y(),mi.z());
+ pi=btVector3(mi.x(),mi.y(),mx.z());break;
+ case (0+0+4): px=btVector3(mi.x(),mi.y(),mx.z());
+ pi=btVector3(mx.x(),mx.y(),mi.z());break;
+ case (1+0+4): px=btVector3(mx.x(),mi.y(),mx.z());
+ pi=btVector3(mi.x(),mx.y(),mi.z());break;
+ case (0+2+4): px=btVector3(mi.x(),mx.y(),mx.z());
+ pi=btVector3(mx.x(),mi.y(),mi.z());break;
+ case (1+2+4): px=btVector3(mx.x(),mx.y(),mx.z());
+ pi=btVector3(mi.x(),mi.y(),mi.z());break;
+ }
+ if((btDot(n,px)+o)<0) return(-1);
+ if((btDot(n,pi)+o)>=0) return(+1);
+ return(0);
+}
+
+//
+DBVT_INLINE btScalar btDbvtAabbMm::ProjectMinimum(const btVector3& v,unsigned signs) const
+{
+ const btVector3* b[]={&mx,&mi};
+ const btVector3 p( b[(signs>>0)&1]->x(),
+ b[(signs>>1)&1]->y(),
+ b[(signs>>2)&1]->z());
+ return(btDot(p,v));
+}
+
+//
+DBVT_INLINE void btDbvtAabbMm::AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const
+{
+ for(int i=0;i<3;++i)
+ {
+ if(d[i]<0)
+ { smi+=mx[i]*d[i];smx+=mi[i]*d[i]; }
+ else
+ { smi+=mi[i]*d[i];smx+=mx[i]*d[i]; }
+ }
+}
+
+//
+DBVT_INLINE bool Intersect( const btDbvtAabbMm& a,
+ const btDbvtAabbMm& b)
+{
+#if DBVT_INT0_IMPL == DBVT_IMPL_SSE
+ const __m128 rt(_mm_or_ps( _mm_cmplt_ps(_mm_load_ps(b.mx),_mm_load_ps(a.mi)),
+ _mm_cmplt_ps(_mm_load_ps(a.mx),_mm_load_ps(b.mi))));
+ const __int32* pu((const __int32*)&rt);
+ return((pu[0]|pu[1]|pu[2])==0);
+#else
+ return( (a.mi.x()<=b.mx.x())&&
+ (a.mx.x()>=b.mi.x())&&
+ (a.mi.y()<=b.mx.y())&&
+ (a.mx.y()>=b.mi.y())&&
+ (a.mi.z()<=b.mx.z())&&
+ (a.mx.z()>=b.mi.z()));
+#endif
+}
+
+
+
+//
+DBVT_INLINE bool Intersect( const btDbvtAabbMm& a,
+ const btVector3& b)
+{
+ return( (b.x()>=a.mi.x())&&
+ (b.y()>=a.mi.y())&&
+ (b.z()>=a.mi.z())&&
+ (b.x()<=a.mx.x())&&
+ (b.y()<=a.mx.y())&&
+ (b.z()<=a.mx.z()));
+}
+
+
+
+
+
+//////////////////////////////////////
+
+
+//
+DBVT_INLINE btScalar Proximity( const btDbvtAabbMm& a,
+ const btDbvtAabbMm& b)
+{
+ const btVector3 d=(a.mi+a.mx)-(b.mi+b.mx);
+ return(btFabs(d.x())+btFabs(d.y())+btFabs(d.z()));
+}
+
+
+
+//
+DBVT_INLINE int Select( const btDbvtAabbMm& o,
+ const btDbvtAabbMm& a,
+ const btDbvtAabbMm& b)
+{
+#if DBVT_SELECT_IMPL == DBVT_IMPL_SSE
+ static ATTRIBUTE_ALIGNED16(const unsigned __int32) mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x7fffffff};
+ ///@todo: the intrinsic version is 11% slower
+#if DBVT_USE_INTRINSIC_SSE
+
+ union btSSEUnion ///NOTE: if we use more intrinsics, move btSSEUnion into the LinearMath directory
+ {
+ __m128 ssereg;
+ float floats[4];
+ int ints[4];
+ };
+
+ __m128 omi(_mm_load_ps(o.mi));
+ omi=_mm_add_ps(omi,_mm_load_ps(o.mx));
+ __m128 ami(_mm_load_ps(a.mi));
+ ami=_mm_add_ps(ami,_mm_load_ps(a.mx));
+ ami=_mm_sub_ps(ami,omi);
+ ami=_mm_and_ps(ami,_mm_load_ps((const float*)mask));
+ __m128 bmi(_mm_load_ps(b.mi));
+ bmi=_mm_add_ps(bmi,_mm_load_ps(b.mx));
+ bmi=_mm_sub_ps(bmi,omi);
+ bmi=_mm_and_ps(bmi,_mm_load_ps((const float*)mask));
+ __m128 t0(_mm_movehl_ps(ami,ami));
+ ami=_mm_add_ps(ami,t0);
+ ami=_mm_add_ss(ami,_mm_shuffle_ps(ami,ami,1));
+ __m128 t1(_mm_movehl_ps(bmi,bmi));
+ bmi=_mm_add_ps(bmi,t1);
+ bmi=_mm_add_ss(bmi,_mm_shuffle_ps(bmi,bmi,1));
+
+ btSSEUnion tmp;
+ tmp.ssereg = _mm_cmple_ss(bmi,ami);
+ return tmp.ints[0]&1;
+
+#else
+ ATTRIBUTE_ALIGNED16(__int32 r[1]);
+ __asm
+ {
+ mov eax,o
+ mov ecx,a
+ mov edx,b
+ movaps xmm0,[eax]
+ movaps xmm5,mask
+ addps xmm0,[eax+16]
+ movaps xmm1,[ecx]
+ movaps xmm2,[edx]
+ addps xmm1,[ecx+16]
+ addps xmm2,[edx+16]
+ subps xmm1,xmm0
+ subps xmm2,xmm0
+ andps xmm1,xmm5
+ andps xmm2,xmm5
+ movhlps xmm3,xmm1
+ movhlps xmm4,xmm2
+ addps xmm1,xmm3
+ addps xmm2,xmm4
+ pshufd xmm3,xmm1,1
+ pshufd xmm4,xmm2,1
+ addss xmm1,xmm3
+ addss xmm2,xmm4
+ cmpless xmm2,xmm1
+ movss r,xmm2
+ }
+ return(r[0]&1);
+#endif
+#else
+ return(Proximity(o,a)<Proximity(o,b)?0:1);
+#endif
+}
+
+//
+DBVT_INLINE void Merge( const btDbvtAabbMm& a,
+ const btDbvtAabbMm& b,
+ btDbvtAabbMm& r)
+{
+#if DBVT_MERGE_IMPL==DBVT_IMPL_SSE
+ __m128 ami(_mm_load_ps(a.mi));
+ __m128 amx(_mm_load_ps(a.mx));
+ __m128 bmi(_mm_load_ps(b.mi));
+ __m128 bmx(_mm_load_ps(b.mx));
+ ami=_mm_min_ps(ami,bmi);
+ amx=_mm_max_ps(amx,bmx);
+ _mm_store_ps(r.mi,ami);
+ _mm_store_ps(r.mx,amx);
+#else
+ for(int i=0;i<3;++i)
+ {
+ if(a.mi[i]<b.mi[i]) r.mi[i]=a.mi[i]; else r.mi[i]=b.mi[i];
+ if(a.mx[i]>b.mx[i]) r.mx[i]=a.mx[i]; else r.mx[i]=b.mx[i];
+ }
+#endif
+}
+
+//
+DBVT_INLINE bool NotEqual( const btDbvtAabbMm& a,
+ const btDbvtAabbMm& b)
+{
+ return( (a.mi.x()!=b.mi.x())||
+ (a.mi.y()!=b.mi.y())||
+ (a.mi.z()!=b.mi.z())||
+ (a.mx.x()!=b.mx.x())||
+ (a.mx.y()!=b.mx.y())||
+ (a.mx.z()!=b.mx.z()));
+}
+
+//
+// Inline's
+//
+
+//
+DBVT_PREFIX
+inline void btDbvt::enumNodes( const btDbvtNode* root,
+ DBVT_IPOLICY)
+{
+ DBVT_CHECKTYPE
+ policy.Process(root);
+ if(root->isinternal())
+ {
+ enumNodes(root->childs[0],policy);
+ enumNodes(root->childs[1],policy);
+ }
+}
+
+//
+DBVT_PREFIX
+inline void btDbvt::enumLeaves( const btDbvtNode* root,
+ DBVT_IPOLICY)
+{
+ DBVT_CHECKTYPE
+ if(root->isinternal())
+ {
+ enumLeaves(root->childs[0],policy);
+ enumLeaves(root->childs[1],policy);
+ }
+ else
+ {
+ policy.Process(root);
+ }
+}
+
+//
+DBVT_PREFIX
+inline void btDbvt::collideTT( const btDbvtNode* root0,
+ const btDbvtNode* root1,
+ DBVT_IPOLICY)
+{
+ DBVT_CHECKTYPE
+ if(root0&&root1)
+ {
+ int depth=1;
+ int treshold=DOUBLE_STACKSIZE-4;
+ btAlignedObjectArray<sStkNN> stkStack;
+ stkStack.resize(DOUBLE_STACKSIZE);
+ stkStack[0]=sStkNN(root0,root1);
+ do {
+ sStkNN p=stkStack[--depth];
+ if(depth>treshold)
+ {
+ stkStack.resize(stkStack.size()*2);
+ treshold=stkStack.size()-4;
+ }
+ if(p.a==p.b)
+ {
+ if(p.a->isinternal())
+ {
+ stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]);
+ stkStack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]);
+ stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]);
+ }
+ }
+ else if(Intersect(p.a->volume,p.b->volume))
+ {
+ if(p.a->isinternal())
+ {
+ if(p.b->isinternal())
+ {
+ stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]);
+ stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]);
+ stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]);
+ stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]);
+ }
+ else
+ {
+ stkStack[depth++]=sStkNN(p.a->childs[0],p.b);
+ stkStack[depth++]=sStkNN(p.a->childs[1],p.b);
+ }
+ }
+ else
+ {
+ if(p.b->isinternal())
+ {
+ stkStack[depth++]=sStkNN(p.a,p.b->childs[0]);
+ stkStack[depth++]=sStkNN(p.a,p.b->childs[1]);
+ }
+ else
+ {
+ policy.Process(p.a,p.b);
+ }
+ }
+ }
+ } while(depth);
+ }
+}
+
+
+
+DBVT_PREFIX
+inline void btDbvt::collideTTpersistentStack( const btDbvtNode* root0,
+ const btDbvtNode* root1,
+ DBVT_IPOLICY)
+{
+ DBVT_CHECKTYPE
+ if(root0&&root1)
+ {
+ int depth=1;
+ int treshold=DOUBLE_STACKSIZE-4;
+
+ m_stkStack.resize(DOUBLE_STACKSIZE);
+ m_stkStack[0]=sStkNN(root0,root1);
+ do {
+ sStkNN p=m_stkStack[--depth];
+ if(depth>treshold)
+ {
+ m_stkStack.resize(m_stkStack.size()*2);
+ treshold=m_stkStack.size()-4;
+ }
+ if(p.a==p.b)
+ {
+ if(p.a->isinternal())
+ {
+ m_stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]);
+ m_stkStack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]);
+ m_stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]);
+ }
+ }
+ else if(Intersect(p.a->volume,p.b->volume))
+ {
+ if(p.a->isinternal())
+ {
+ if(p.b->isinternal())
+ {
+ m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]);
+ m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]);
+ m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]);
+ m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]);
+ }
+ else
+ {
+ m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b);
+ m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b);
+ }
+ }
+ else
+ {
+ if(p.b->isinternal())
+ {
+ m_stkStack[depth++]=sStkNN(p.a,p.b->childs[0]);
+ m_stkStack[depth++]=sStkNN(p.a,p.b->childs[1]);
+ }
+ else
+ {
+ policy.Process(p.a,p.b);
+ }
+ }
+ }
+ } while(depth);
+ }
+}
+
+#if 0
+//
+DBVT_PREFIX
+inline void btDbvt::collideTT( const btDbvtNode* root0,
+ const btDbvtNode* root1,
+ const btTransform& xform,
+ DBVT_IPOLICY)
+{
+ DBVT_CHECKTYPE
+ if(root0&&root1)
+ {
+ int depth=1;
+ int treshold=DOUBLE_STACKSIZE-4;
+ btAlignedObjectArray<sStkNN> stkStack;
+ stkStack.resize(DOUBLE_STACKSIZE);
+ stkStack[0]=sStkNN(root0,root1);
+ do {
+ sStkNN p=stkStack[--depth];
+ if(Intersect(p.a->volume,p.b->volume,xform))
+ {
+ if(depth>treshold)
+ {
+ stkStack.resize(stkStack.size()*2);
+ treshold=stkStack.size()-4;
+ }
+ if(p.a->isinternal())
+ {
+ if(p.b->isinternal())
+ {
+ stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]);
+ stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]);
+ stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]);
+ stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]);
+ }
+ else
+ {
+ stkStack[depth++]=sStkNN(p.a->childs[0],p.b);
+ stkStack[depth++]=sStkNN(p.a->childs[1],p.b);
+ }
+ }
+ else
+ {
+ if(p.b->isinternal())
+ {
+ stkStack[depth++]=sStkNN(p.a,p.b->childs[0]);
+ stkStack[depth++]=sStkNN(p.a,p.b->childs[1]);
+ }
+ else
+ {
+ policy.Process(p.a,p.b);
+ }
+ }
+ }
+ } while(depth);
+ }
+}
+//
+DBVT_PREFIX
+inline void btDbvt::collideTT( const btDbvtNode* root0,
+ const btTransform& xform0,
+ const btDbvtNode* root1,
+ const btTransform& xform1,
+ DBVT_IPOLICY)
+{
+ const btTransform xform=xform0.inverse()*xform1;
+ collideTT(root0,root1,xform,policy);
+}
+#endif
+
+//
+DBVT_PREFIX
+inline void btDbvt::collideTV( const btDbvtNode* root,
+ const btDbvtVolume& vol,
+ DBVT_IPOLICY)
+{
+ DBVT_CHECKTYPE
+ if(root)
+ {
+ ATTRIBUTE_ALIGNED16(btDbvtVolume) volume(vol);
+ btAlignedObjectArray<const btDbvtNode*> stack;
+ stack.resize(0);
+ stack.reserve(SIMPLE_STACKSIZE);
+ stack.push_back(root);
+ do {
+ const btDbvtNode* n=stack[stack.size()-1];
+ stack.pop_back();
+ if(Intersect(n->volume,volume))
+ {
+ if(n->isinternal())
+ {
+ stack.push_back(n->childs[0]);
+ stack.push_back(n->childs[1]);
+ }
+ else
+ {
+ policy.Process(n);
+ }
+ }
+ } while(stack.size()>0);
+ }
+}
+
+DBVT_PREFIX
+inline void btDbvt::rayTestInternal( const btDbvtNode* root,
+ const btVector3& rayFrom,
+ const btVector3& rayTo,
+ const btVector3& rayDirectionInverse,
+ unsigned int signs[3],
+ btScalar lambda_max,
+ const btVector3& aabbMin,
+ const btVector3& aabbMax,
+ DBVT_IPOLICY) const
+{
+ DBVT_CHECKTYPE
+ if(root)
+ {
+ btVector3 resultNormal;
+
+ int depth=1;
+ int treshold=DOUBLE_STACKSIZE-2;
+ btAlignedObjectArray<const btDbvtNode*> stack;
+ stack.resize(DOUBLE_STACKSIZE);
+ stack[0]=root;
+ btVector3 bounds[2];
+ do
+ {
+ const btDbvtNode* node=stack[--depth];
+ bounds[0] = node->volume.Mins()+aabbMin;
+ bounds[1] = node->volume.Maxs()+aabbMax;
+ btScalar tmin=1.f,lambda_min=0.f;
+ unsigned int result1=false;
+ result1 = btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max);
+ if(result1)
+ {
+ if(node->isinternal())
+ {
+ if(depth>treshold)
+ {
+ stack.resize(stack.size()*2);
+ treshold=stack.size()-2;
+ }
+ stack[depth++]=node->childs[0];
+ stack[depth++]=node->childs[1];
+ }
+ else
+ {
+ policy.Process(node);
+ }
+ }
+ } while(depth);
+ }
+}
+
+//
+DBVT_PREFIX
+inline void btDbvt::rayTest( const btDbvtNode* root,
+ const btVector3& rayFrom,
+ const btVector3& rayTo,
+ DBVT_IPOLICY)
+{
+ DBVT_CHECKTYPE
+ if(root)
+ {
+ btVector3 rayDir = (rayTo-rayFrom);
+ rayDir.normalize ();
+
+ ///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT
+ btVector3 rayDirectionInverse;
+ rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0];
+ rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1];
+ rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2];
+ unsigned int signs[3] = { rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0};
+
+ btScalar lambda_max = rayDir.dot(rayTo-rayFrom);
+
+ btVector3 resultNormal;
+
+ btAlignedObjectArray<const btDbvtNode*> stack;
+
+ int depth=1;
+ int treshold=DOUBLE_STACKSIZE-2;
+
+ stack.resize(DOUBLE_STACKSIZE);
+ stack[0]=root;
+ btVector3 bounds[2];
+ do {
+ const btDbvtNode* node=stack[--depth];
+
+ bounds[0] = node->volume.Mins();
+ bounds[1] = node->volume.Maxs();
+
+ btScalar tmin=1.f,lambda_min=0.f;
+ unsigned int result1 = btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max);
+
+#ifdef COMPARE_BTRAY_AABB2
+ btScalar param=1.f;
+ bool result2 = btRayAabb(rayFrom,rayTo,node->volume.Mins(),node->volume.Maxs(),param,resultNormal);
+ btAssert(result1 == result2);
+#endif //TEST_BTRAY_AABB2
+
+ if(result1)
+ {
+ if(node->isinternal())
+ {
+ if(depth>treshold)
+ {
+ stack.resize(stack.size()*2);
+ treshold=stack.size()-2;
+ }
+ stack[depth++]=node->childs[0];
+ stack[depth++]=node->childs[1];
+ }
+ else
+ {
+ policy.Process(node);
+ }
+ }
+ } while(depth);
+
+ }
+}
+
+//
+DBVT_PREFIX
+inline void btDbvt::collideKDOP(const btDbvtNode* root,
+ const btVector3* normals,
+ const btScalar* offsets,
+ int count,
+ DBVT_IPOLICY)
+{
+ DBVT_CHECKTYPE
+ if(root)
+ {
+ const int inside=(1<<count)-1;
+ btAlignedObjectArray<sStkNP> stack;
+ int signs[sizeof(unsigned)*8];
+ btAssert(count<int (sizeof(signs)/sizeof(signs[0])));
+ for(int i=0;i<count;++i)
+ {
+ signs[i]= ((normals[i].x()>=0)?1:0)+
+ ((normals[i].y()>=0)?2:0)+
+ ((normals[i].z()>=0)?4:0);
+ }
+ stack.reserve(SIMPLE_STACKSIZE);
+ stack.push_back(sStkNP(root,0));
+ do {
+ sStkNP se=stack[stack.size()-1];
+ bool out=false;
+ stack.pop_back();
+ for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1)
+ {
+ if(0==(se.mask&j))
+ {
+ const int side=se.node->volume.Classify(normals[i],offsets[i],signs[i]);
+ switch(side)
+ {
+ case -1: out=true;break;
+ case +1: se.mask|=j;break;
+ }
+ }
+ }
+ if(!out)
+ {
+ if((se.mask!=inside)&&(se.node->isinternal()))
+ {
+ stack.push_back(sStkNP(se.node->childs[0],se.mask));
+ stack.push_back(sStkNP(se.node->childs[1],se.mask));
+ }
+ else
+ {
+ if(policy.AllLeaves(se.node)) enumLeaves(se.node,policy);
+ }
+ }
+ } while(stack.size());
+ }
+}
+
+//
+DBVT_PREFIX
+inline void btDbvt::collideOCL( const btDbvtNode* root,
+ const btVector3* normals,
+ const btScalar* offsets,
+ const btVector3& sortaxis,
+ int count,
+ DBVT_IPOLICY,
+ bool fsort)
+{
+ DBVT_CHECKTYPE
+ if(root)
+ {
+ const unsigned srtsgns=(sortaxis[0]>=0?1:0)+
+ (sortaxis[1]>=0?2:0)+
+ (sortaxis[2]>=0?4:0);
+ const int inside=(1<<count)-1;
+ btAlignedObjectArray<sStkNPS> stock;
+ btAlignedObjectArray<int> ifree;
+ btAlignedObjectArray<int> stack;
+ int signs[sizeof(unsigned)*8];
+ btAssert(count<int (sizeof(signs)/sizeof(signs[0])));
+ for(int i=0;i<count;++i)
+ {
+ signs[i]= ((normals[i].x()>=0)?1:0)+
+ ((normals[i].y()>=0)?2:0)+
+ ((normals[i].z()>=0)?4:0);
+ }
+ stock.reserve(SIMPLE_STACKSIZE);
+ stack.reserve(SIMPLE_STACKSIZE);
+ ifree.reserve(SIMPLE_STACKSIZE);
+ stack.push_back(allocate(ifree,stock,sStkNPS(root,0,root->volume.ProjectMinimum(sortaxis,srtsgns))));
+ do {
+ const int id=stack[stack.size()-1];
+ sStkNPS se=stock[id];
+ stack.pop_back();ifree.push_back(id);
+ if(se.mask!=inside)
+ {
+ bool out=false;
+ for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1)
+ {
+ if(0==(se.mask&j))
+ {
+ const int side=se.node->volume.Classify(normals[i],offsets[i],signs[i]);
+ switch(side)
+ {
+ case -1: out=true;break;
+ case +1: se.mask|=j;break;
+ }
+ }
+ }
+ if(out) continue;
+ }
+ if(policy.Descent(se.node))
+ {
+ if(se.node->isinternal())
+ {
+ const btDbvtNode* pns[]={ se.node->childs[0],se.node->childs[1]};
+ sStkNPS nes[]={ sStkNPS(pns[0],se.mask,pns[0]->volume.ProjectMinimum(sortaxis,srtsgns)),
+ sStkNPS(pns[1],se.mask,pns[1]->volume.ProjectMinimum(sortaxis,srtsgns))};
+ const int q=nes[0].value<nes[1].value?1:0;
+ int j=stack.size();
+ if(fsort&&(j>0))
+ {
+ /* Insert 0 */
+ j=nearest(&stack[0],&stock[0],nes[q].value,0,stack.size());
+ stack.push_back(0);
+#if DBVT_USE_MEMMOVE
+ memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1));
+#else
+ for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1];
+#endif
+ stack[j]=allocate(ifree,stock,nes[q]);
+ /* Insert 1 */
+ j=nearest(&stack[0],&stock[0],nes[1-q].value,j,stack.size());
+ stack.push_back(0);
+#if DBVT_USE_MEMMOVE
+ memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1));
+#else
+ for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1];
+#endif
+ stack[j]=allocate(ifree,stock,nes[1-q]);
+ }
+ else
+ {
+ stack.push_back(allocate(ifree,stock,nes[q]));
+ stack.push_back(allocate(ifree,stock,nes[1-q]));
+ }
+ }
+ else
+ {
+ policy.Process(se.node,se.value);
+ }
+ }
+ } while(stack.size());
+ }
+}
+
+//
+DBVT_PREFIX
+inline void btDbvt::collideTU( const btDbvtNode* root,
+ DBVT_IPOLICY)
+{
+ DBVT_CHECKTYPE
+ if(root)
+ {
+ btAlignedObjectArray<const btDbvtNode*> stack;
+ stack.reserve(SIMPLE_STACKSIZE);
+ stack.push_back(root);
+ do {
+ const btDbvtNode* n=stack[stack.size()-1];
+ stack.pop_back();
+ if(policy.Descent(n))
+ {
+ if(n->isinternal())
+ { stack.push_back(n->childs[0]);stack.push_back(n->childs[1]); }
+ else
+ { policy.Process(n); }
+ }
+ } while(stack.size()>0);
+ }
+}
+
+//
+// PP Cleanup
+//
+
+#undef DBVT_USE_MEMMOVE
+#undef DBVT_USE_TEMPLATE
+#undef DBVT_VIRTUAL_DTOR
+#undef DBVT_VIRTUAL
+#undef DBVT_PREFIX
+#undef DBVT_IPOLICY
+#undef DBVT_CHECKTYPE
+#undef DBVT_IMPL_GENERIC
+#undef DBVT_IMPL_SSE
+#undef DBVT_USE_INTRINSIC_SSE
+#undef DBVT_SELECT_IMPL
+#undef DBVT_MERGE_IMPL
+#undef DBVT_INT0_IMPL
+
+#endif
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,135 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///btDbvtBroadphase implementation by Nathanael Presson
+#ifndef BT_DBVT_BROADPHASE_H
+#define BT_DBVT_BROADPHASE_H
+
+#include "BulletCollision/BroadphaseCollision/btDbvt.h"
+#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h"
+
+//
+// Compile time config
+//
+
+#define DBVT_BP_PROFILE 0
+//#define DBVT_BP_SORTPAIRS 1
+#define DBVT_BP_PREVENTFALSEUPDATE 0
+#define DBVT_BP_ACCURATESLEEPING 0
+#define DBVT_BP_ENABLE_BENCHMARK 0
+#define DBVT_BP_MARGIN (btScalar)0.05
+
+#if DBVT_BP_PROFILE
+#define DBVT_BP_PROFILING_RATE 256
+#include "LinearMath/btQuickprof.h"
+#endif
+
+//
+// btDbvtProxy
+//
+struct btDbvtProxy : btBroadphaseProxy
+{
+ /* Fields */
+ //btDbvtAabbMm aabb;
+ btDbvtNode* leaf;
+ btDbvtProxy* links[2];
+ int stage;
+ /* ctor */
+ btDbvtProxy(const btVector3& aabbMin,const btVector3& aabbMax,void* userPtr,short int collisionFilterGroup, short int collisionFilterMask) :
+ btBroadphaseProxy(aabbMin,aabbMax,userPtr,collisionFilterGroup,collisionFilterMask)
+ {
+ links[0]=links[1]=0;
+ }
+};
+
+typedef btAlignedObjectArray<btDbvtProxy*> btDbvtProxyArray;
+
+///The btDbvtBroadphase implements a broadphase using two dynamic AABB bounding volume hierarchies/trees (see btDbvt).
+///One tree is used for static/non-moving objects, and another tree is used for dynamic objects. Objects can move from one tree to the other.
+///This is a very fast broadphase, especially for very dynamic worlds where many objects are moving. Its insert/add and remove of objects is generally faster than the sweep and prune broadphases btAxisSweep3 and bt32BitAxisSweep3.
+struct btDbvtBroadphase : btBroadphaseInterface
+{
+ /* Config */
+ enum {
+ DYNAMIC_SET = 0, /* Dynamic set index */
+ FIXED_SET = 1, /* Fixed set index */
+ STAGECOUNT = 2 /* Number of stages */
+ };
+ /* Fields */
+ btDbvt m_sets[2]; // Dbvt sets
+ btDbvtProxy* m_stageRoots[STAGECOUNT+1]; // Stages list
+ btOverlappingPairCache* m_paircache; // Pair cache
+ btScalar m_prediction; // Velocity prediction
+ int m_stageCurrent; // Current stage
+ int m_fupdates; // % of fixed updates per frame
+ int m_dupdates; // % of dynamic updates per frame
+ int m_cupdates; // % of cleanup updates per frame
+ int m_newpairs; // Number of pairs created
+ int m_fixedleft; // Fixed optimization left
+ unsigned m_updates_call; // Number of updates call
+ unsigned m_updates_done; // Number of updates done
+ btScalar m_updates_ratio; // m_updates_done/m_updates_call
+ int m_pid; // Parse id
+ int m_cid; // Cleanup index
+ int m_gid; // Gen id
+ bool m_releasepaircache; // Release pair cache on delete
+ bool m_deferedcollide; // Defere dynamic/static collision to collide call
+ bool m_needcleanup; // Need to run cleanup?
+#if DBVT_BP_PROFILE
+ btClock m_clock;
+ struct {
+ unsigned long m_total;
+ unsigned long m_ddcollide;
+ unsigned long m_fdcollide;
+ unsigned long m_cleanup;
+ unsigned long m_jobcount;
+ } m_profiling;
+#endif
+ /* Methods */
+ btDbvtBroadphase(btOverlappingPairCache* paircache=0);
+ ~btDbvtBroadphase();
+ void collide(btDispatcher* dispatcher);
+ void optimize();
+ /* btBroadphaseInterface Implementation */
+ btBroadphaseProxy* createProxy(const btVector3& aabbMin,const btVector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy);
+ void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
+ void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher);
+ virtual void rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin=btVector3(0,0,0), const btVector3& aabbMax = btVector3(0,0,0));
+
+ virtual void getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const;
+ void calculateOverlappingPairs(btDispatcher* dispatcher);
+ btOverlappingPairCache* getOverlappingPairCache();
+ const btOverlappingPairCache* getOverlappingPairCache() const;
+ void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const;
+ void printStats();
+ static void benchmark(btBroadphaseInterface*);
+
+ void setVelocityPrediction(btScalar prediction)
+ {
+ m_prediction = prediction;
+ }
+ btScalar getVelocityPrediction() const
+ {
+ return m_prediction;
+ }
+
+ void performDeferredRemoval(btDispatcher* dispatcher);
+
+ ///reset broadphase internal structures, to ensure determinism/reproducability
+ virtual void resetPool(btDispatcher* dispatcher);
+
+};
+
+#endif
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btDispatcher.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btDispatcher.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btDispatcher.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btDispatcher.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,106 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef _DISPATCHER_H
+#define _DISPATCHER_H
+
+#include "LinearMath/btScalar.h"
+
+class btCollisionAlgorithm;
+struct btBroadphaseProxy;
+class btRigidBody;
+class btCollisionObject;
+class btOverlappingPairCache;
+
+
+class btPersistentManifold;
+class btStackAlloc;
+
+struct btDispatcherInfo
+{
+ enum DispatchFunc
+ {
+ DISPATCH_DISCRETE = 1,
+ DISPATCH_CONTINUOUS
+ };
+ btDispatcherInfo()
+ :m_timeStep(btScalar(0.)),
+ m_stepCount(0),
+ m_dispatchFunc(DISPATCH_DISCRETE),
+ m_timeOfImpact(btScalar(1.)),
+ m_useContinuous(false),
+ m_debugDraw(0),
+ m_enableSatConvex(false),
+ m_enableSPU(true),
+ m_useEpa(true),
+ m_allowedCcdPenetration(btScalar(0.04)),
+ m_useConvexConservativeDistanceUtil(false),
+ m_convexConservativeDistanceThreshold(0.0f),
+ m_stackAllocator(0)
+ {
+
+ }
+ btScalar m_timeStep;
+ int m_stepCount;
+ int m_dispatchFunc;
+ mutable btScalar m_timeOfImpact;
+ bool m_useContinuous;
+ class btIDebugDraw* m_debugDraw;
+ bool m_enableSatConvex;
+ bool m_enableSPU;
+ bool m_useEpa;
+ btScalar m_allowedCcdPenetration;
+ bool m_useConvexConservativeDistanceUtil;
+ btScalar m_convexConservativeDistanceThreshold;
+ btStackAlloc* m_stackAllocator;
+};
+
+///The btDispatcher interface class can be used in combination with broadphase to dispatch calculations for overlapping pairs.
+///For example for pairwise collision detection, calculating contact points stored in btPersistentManifold or user callbacks (game logic).
+class btDispatcher
+{
+
+
+public:
+ virtual ~btDispatcher() ;
+
+ virtual btCollisionAlgorithm* findAlgorithm(btCollisionObject* body0,btCollisionObject* body1,btPersistentManifold* sharedManifold=0) = 0;
+
+ virtual btPersistentManifold* getNewManifold(void* body0,void* body1)=0;
+
+ virtual void releaseManifold(btPersistentManifold* manifold)=0;
+
+ virtual void clearManifold(btPersistentManifold* manifold)=0;
+
+ virtual bool needsCollision(btCollisionObject* body0,btCollisionObject* body1) = 0;
+
+ virtual bool needsResponse(btCollisionObject* body0,btCollisionObject* body1)=0;
+
+ virtual void dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,const btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher) =0;
+
+ virtual int getNumManifolds() const = 0;
+
+ virtual btPersistentManifold* getManifoldByIndexInternal(int index) = 0;
+
+ virtual btPersistentManifold** getInternalManifoldPointer() = 0;
+
+ virtual void* allocateCollisionAlgorithm(int size) = 0;
+
+ virtual void freeCollisionAlgorithm(void* ptr) = 0;
+
+};
+
+
+#endif //_DISPATCHER_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,151 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+#ifndef BT_MULTI_SAP_BROADPHASE
+#define BT_MULTI_SAP_BROADPHASE
+
+#include "btBroadphaseInterface.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "btOverlappingPairCache.h"
+
+
+class btBroadphaseInterface;
+class btSimpleBroadphase;
+
+
+typedef btAlignedObjectArray<btBroadphaseInterface*> btSapBroadphaseArray;
+
+///The btMultiSapBroadphase is a research project, not recommended to use in production. Use btAxisSweep3 or btDbvtBroadphase instead.
+///The btMultiSapBroadphase is a broadphase that contains multiple SAP broadphases.
+///The user can add SAP broadphases that cover the world. A btBroadphaseProxy can be in multiple child broadphases at the same time.
+///A btQuantizedBvh acceleration structures finds overlapping SAPs for each btBroadphaseProxy.
+///See http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=328
+///and http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1329
+class btMultiSapBroadphase :public btBroadphaseInterface
+{
+ btSapBroadphaseArray m_sapBroadphases;
+
+ btSimpleBroadphase* m_simpleBroadphase;
+
+ btOverlappingPairCache* m_overlappingPairs;
+
+ class btQuantizedBvh* m_optimizedAabbTree;
+
+
+ bool m_ownsPairCache;
+
+ btOverlapFilterCallback* m_filterCallback;
+
+ int m_invalidPair;
+
+ struct btBridgeProxy
+ {
+ btBroadphaseProxy* m_childProxy;
+ btBroadphaseInterface* m_childBroadphase;
+ };
+
+
+public:
+
+ struct btMultiSapProxy : public btBroadphaseProxy
+ {
+
+ ///array with all the entries that this proxy belongs to
+ btAlignedObjectArray<btBridgeProxy*> m_bridgeProxies;
+ btVector3 m_aabbMin;
+ btVector3 m_aabbMax;
+
+ int m_shapeType;
+
+/* void* m_userPtr;
+ short int m_collisionFilterGroup;
+ short int m_collisionFilterMask;
+*/
+ btMultiSapProxy(const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask)
+ :btBroadphaseProxy(aabbMin,aabbMax,userPtr,collisionFilterGroup,collisionFilterMask),
+ m_aabbMin(aabbMin),
+ m_aabbMax(aabbMax),
+ m_shapeType(shapeType)
+ {
+ m_multiSapParentProxy =this;
+ }
+
+
+ };
+
+protected:
+
+
+ btAlignedObjectArray<btMultiSapProxy*> m_multiSapProxies;
+
+public:
+
+ btMultiSapBroadphase(int maxProxies = 16384,btOverlappingPairCache* pairCache=0);
+
+
+ btSapBroadphaseArray& getBroadphaseArray()
+ {
+ return m_sapBroadphases;
+ }
+
+ const btSapBroadphaseArray& getBroadphaseArray() const
+ {
+ return m_sapBroadphases;
+ }
+
+ virtual ~btMultiSapBroadphase();
+
+ virtual btBroadphaseProxy* createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* multiSapProxy);
+ virtual void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
+ virtual void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher);
+ virtual void getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const;
+
+ virtual void rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback,const btVector3& aabbMin=btVector3(0,0,0),const btVector3& aabbMax=btVector3(0,0,0));
+
+ void addToChildBroadphase(btMultiSapProxy* parentMultiSapProxy, btBroadphaseProxy* childProxy, btBroadphaseInterface* childBroadphase);
+
+ ///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb
+ virtual void calculateOverlappingPairs(btDispatcher* dispatcher);
+
+ bool testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
+
+ virtual btOverlappingPairCache* getOverlappingPairCache()
+ {
+ return m_overlappingPairs;
+ }
+ virtual const btOverlappingPairCache* getOverlappingPairCache() const
+ {
+ return m_overlappingPairs;
+ }
+
+ ///getAabb returns the axis aligned bounding box in the 'global' coordinate frame
+ ///will add some transform later
+ virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const
+ {
+ aabbMin.setValue(-BT_LARGE_FLOAT,-BT_LARGE_FLOAT,-BT_LARGE_FLOAT);
+ aabbMax.setValue(BT_LARGE_FLOAT,BT_LARGE_FLOAT,BT_LARGE_FLOAT);
+ }
+
+ void buildTree(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax);
+
+ virtual void printStats();
+
+ void quicksort (btBroadphasePairArray& a, int lo, int hi);
+
+ ///reset broadphase internal structures, to ensure determinism/reproducability
+ virtual void resetPool(btDispatcher* dispatcher);
+
+};
+
+#endif //BT_MULTI_SAP_BROADPHASE
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,468 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef OVERLAPPING_PAIR_CACHE_H
+#define OVERLAPPING_PAIR_CACHE_H
+
+
+#include "btBroadphaseInterface.h"
+#include "btBroadphaseProxy.h"
+#include "btOverlappingPairCallback.h"
+
+#include "LinearMath/btAlignedObjectArray.h"
+class btDispatcher;
+
+typedef btAlignedObjectArray<btBroadphasePair> btBroadphasePairArray;
+
+struct btOverlapCallback
+{
+ virtual ~btOverlapCallback()
+ {}
+ //return true for deletion of the pair
+ virtual bool processOverlap(btBroadphasePair& pair) = 0;
+
+};
+
+struct btOverlapFilterCallback
+{
+ virtual ~btOverlapFilterCallback()
+ {}
+ // return true when pairs need collision
+ virtual bool needBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const = 0;
+};
+
+
+
+
+
+
+
+extern int gRemovePairs;
+extern int gAddedPairs;
+extern int gFindPairs;
+
+const int BT_NULL_PAIR=0xffffffff;
+
+///The btOverlappingPairCache provides an interface for overlapping pair management (add, remove, storage), used by the btBroadphaseInterface broadphases.
+///The btHashedOverlappingPairCache and btSortedOverlappingPairCache classes are two implementations.
+class btOverlappingPairCache : public btOverlappingPairCallback
+{
+public:
+ virtual ~btOverlappingPairCache() {} // this is needed so we can get to the derived class destructor
+
+ virtual btBroadphasePair* getOverlappingPairArrayPtr() = 0;
+
+ virtual const btBroadphasePair* getOverlappingPairArrayPtr() const = 0;
+
+ virtual btBroadphasePairArray& getOverlappingPairArray() = 0;
+
+ virtual void cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher) = 0;
+
+ virtual int getNumOverlappingPairs() const = 0;
+
+ virtual void cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher) = 0;
+
+ virtual void setOverlapFilterCallback(btOverlapFilterCallback* callback) = 0;
+
+ virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher) = 0;
+
+ virtual btBroadphasePair* findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) = 0;
+
+ virtual bool hasDeferredRemoval() = 0;
+
+ virtual void setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback)=0;
+
+ virtual void sortOverlappingPairs(btDispatcher* dispatcher) = 0;
+
+
+};
+
+/// Hash-space based Pair Cache, thanks to Erin Catto, Box2D, http://www.box2d.org, and Pierre Terdiman, Codercorner, http://codercorner.com
+class btHashedOverlappingPairCache : public btOverlappingPairCache
+{
+ btBroadphasePairArray m_overlappingPairArray;
+ btOverlapFilterCallback* m_overlapFilterCallback;
+ bool m_blockedForChanges;
+
+
+public:
+ btHashedOverlappingPairCache();
+ virtual ~btHashedOverlappingPairCache();
+
+
+ void removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
+
+ virtual void* removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher);
+
+ SIMD_FORCE_INLINE bool needsBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const
+ {
+ if (m_overlapFilterCallback)
+ return m_overlapFilterCallback->needBroadphaseCollision(proxy0,proxy1);
+
+ bool collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
+ collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
+
+ return collides;
+ }
+
+ // Add a pair and return the new pair. If the pair already exists,
+ // no new pair is created and the old one is returned.
+ virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+ {
+ gAddedPairs++;
+
+ if (!needsBroadphaseCollision(proxy0,proxy1))
+ return 0;
+
+ return internalAddPair(proxy0,proxy1);
+ }
+
+
+
+ void cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
+
+
+ virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher);
+
+ virtual btBroadphasePair* getOverlappingPairArrayPtr()
+ {
+ return &m_overlappingPairArray[0];
+ }
+
+ const btBroadphasePair* getOverlappingPairArrayPtr() const
+ {
+ return &m_overlappingPairArray[0];
+ }
+
+ btBroadphasePairArray& getOverlappingPairArray()
+ {
+ return m_overlappingPairArray;
+ }
+
+ const btBroadphasePairArray& getOverlappingPairArray() const
+ {
+ return m_overlappingPairArray;
+ }
+
+ void cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher);
+
+
+
+ btBroadphasePair* findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1);
+
+ int GetCount() const { return m_overlappingPairArray.size(); }
+// btBroadphasePair* GetPairs() { return m_pairs; }
+
+ btOverlapFilterCallback* getOverlapFilterCallback()
+ {
+ return m_overlapFilterCallback;
+ }
+
+ void setOverlapFilterCallback(btOverlapFilterCallback* callback)
+ {
+ m_overlapFilterCallback = callback;
+ }
+
+ int getNumOverlappingPairs() const
+ {
+ return m_overlappingPairArray.size();
+ }
+private:
+
+ btBroadphasePair* internalAddPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
+
+ void growTables();
+
+ SIMD_FORCE_INLINE bool equalsPair(const btBroadphasePair& pair, int proxyId1, int proxyId2)
+ {
+ return pair.m_pProxy0->getUid() == proxyId1 && pair.m_pProxy1->getUid() == proxyId2;
+ }
+
+ /*
+ // Thomas Wang's hash, see: http://www.concentric.net/~Ttwang/tech/inthash.htm
+ // This assumes proxyId1 and proxyId2 are 16-bit.
+ SIMD_FORCE_INLINE int getHash(int proxyId1, int proxyId2)
+ {
+ int key = (proxyId2 << 16) | proxyId1;
+ key = ~key + (key << 15);
+ key = key ^ (key >> 12);
+ key = key + (key << 2);
+ key = key ^ (key >> 4);
+ key = key * 2057;
+ key = key ^ (key >> 16);
+ return key;
+ }
+ */
+
+
+
+ SIMD_FORCE_INLINE unsigned int getHash(unsigned int proxyId1, unsigned int proxyId2)
+ {
+ int key = static_cast<int>(((unsigned int)proxyId1) | (((unsigned int)proxyId2) <<16));
+ // Thomas Wang's hash
+
+ key += ~(key << 15);
+ key ^= (key >> 10);
+ key += (key << 3);
+ key ^= (key >> 6);
+ key += ~(key << 11);
+ key ^= (key >> 16);
+ return static_cast<unsigned int>(key);
+ }
+
+
+
+
+
+ SIMD_FORCE_INLINE btBroadphasePair* internalFindPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1, int hash)
+ {
+ int proxyId1 = proxy0->getUid();
+ int proxyId2 = proxy1->getUid();
+ #if 0 // wrong, 'equalsPair' use unsorted uids, copy-past devil striked again. Nat.
+ if (proxyId1 > proxyId2)
+ btSwap(proxyId1, proxyId2);
+ #endif
+
+ int index = m_hashTable[hash];
+
+ while( index != BT_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false)
+ {
+ index = m_next[index];
+ }
+
+ if ( index == BT_NULL_PAIR )
+ {
+ return NULL;
+ }
+
+ btAssert(index < m_overlappingPairArray.size());
+
+ return &m_overlappingPairArray[index];
+ }
+
+ virtual bool hasDeferredRemoval()
+ {
+ return false;
+ }
+
+ virtual void setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback)
+ {
+ m_ghostPairCallback = ghostPairCallback;
+ }
+
+ virtual void sortOverlappingPairs(btDispatcher* dispatcher);
+
+
+protected:
+
+ btAlignedObjectArray<int> m_hashTable;
+ btAlignedObjectArray<int> m_next;
+ btOverlappingPairCallback* m_ghostPairCallback;
+
+};
+
+
+
+
+///btSortedOverlappingPairCache maintains the objects with overlapping AABB
+///Typically managed by the Broadphase, Axis3Sweep or btSimpleBroadphase
+class btSortedOverlappingPairCache : public btOverlappingPairCache
+{
+ protected:
+ //avoid brute-force finding all the time
+ btBroadphasePairArray m_overlappingPairArray;
+
+ //during the dispatch, check that user doesn't destroy/create proxy
+ bool m_blockedForChanges;
+
+ ///by default, do the removal during the pair traversal
+ bool m_hasDeferredRemoval;
+
+ //if set, use the callback instead of the built in filter in needBroadphaseCollision
+ btOverlapFilterCallback* m_overlapFilterCallback;
+
+ btOverlappingPairCallback* m_ghostPairCallback;
+
+ public:
+
+ btSortedOverlappingPairCache();
+ virtual ~btSortedOverlappingPairCache();
+
+ virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher);
+
+ void* removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher);
+
+ void cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher);
+
+ btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
+
+ btBroadphasePair* findPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
+
+
+ void cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
+
+ void removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
+
+
+ inline bool needsBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const
+ {
+ if (m_overlapFilterCallback)
+ return m_overlapFilterCallback->needBroadphaseCollision(proxy0,proxy1);
+
+ bool collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
+ collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
+
+ return collides;
+ }
+
+ btBroadphasePairArray& getOverlappingPairArray()
+ {
+ return m_overlappingPairArray;
+ }
+
+ const btBroadphasePairArray& getOverlappingPairArray() const
+ {
+ return m_overlappingPairArray;
+ }
+
+
+
+
+ btBroadphasePair* getOverlappingPairArrayPtr()
+ {
+ return &m_overlappingPairArray[0];
+ }
+
+ const btBroadphasePair* getOverlappingPairArrayPtr() const
+ {
+ return &m_overlappingPairArray[0];
+ }
+
+ int getNumOverlappingPairs() const
+ {
+ return m_overlappingPairArray.size();
+ }
+
+ btOverlapFilterCallback* getOverlapFilterCallback()
+ {
+ return m_overlapFilterCallback;
+ }
+
+ void setOverlapFilterCallback(btOverlapFilterCallback* callback)
+ {
+ m_overlapFilterCallback = callback;
+ }
+
+ virtual bool hasDeferredRemoval()
+ {
+ return m_hasDeferredRemoval;
+ }
+
+ virtual void setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback)
+ {
+ m_ghostPairCallback = ghostPairCallback;
+ }
+
+ virtual void sortOverlappingPairs(btDispatcher* dispatcher);
+
+
+};
+
+
+
+///btNullPairCache skips add/removal of overlapping pairs. Userful for benchmarking and unit testing.
+class btNullPairCache : public btOverlappingPairCache
+{
+
+ btBroadphasePairArray m_overlappingPairArray;
+
+public:
+
+ virtual btBroadphasePair* getOverlappingPairArrayPtr()
+ {
+ return &m_overlappingPairArray[0];
+ }
+ const btBroadphasePair* getOverlappingPairArrayPtr() const
+ {
+ return &m_overlappingPairArray[0];
+ }
+ btBroadphasePairArray& getOverlappingPairArray()
+ {
+ return m_overlappingPairArray;
+ }
+
+ virtual void cleanOverlappingPair(btBroadphasePair& /*pair*/,btDispatcher* /*dispatcher*/)
+ {
+
+ }
+
+ virtual int getNumOverlappingPairs() const
+ {
+ return 0;
+ }
+
+ virtual void cleanProxyFromPairs(btBroadphaseProxy* /*proxy*/,btDispatcher* /*dispatcher*/)
+ {
+
+ }
+
+ virtual void setOverlapFilterCallback(btOverlapFilterCallback* /*callback*/)
+ {
+ }
+
+ virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* /*dispatcher*/)
+ {
+ }
+
+ virtual btBroadphasePair* findPair(btBroadphaseProxy* /*proxy0*/, btBroadphaseProxy* /*proxy1*/)
+ {
+ return 0;
+ }
+
+ virtual bool hasDeferredRemoval()
+ {
+ return true;
+ }
+
+ virtual void setInternalGhostPairCallback(btOverlappingPairCallback* /* ghostPairCallback */)
+ {
+
+ }
+
+ virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* /*proxy0*/,btBroadphaseProxy* /*proxy1*/)
+ {
+ return 0;
+ }
+
+ virtual void* removeOverlappingPair(btBroadphaseProxy* /*proxy0*/,btBroadphaseProxy* /*proxy1*/,btDispatcher* /*dispatcher*/)
+ {
+ return 0;
+ }
+
+ virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy* /*proxy0*/,btDispatcher* /*dispatcher*/)
+ {
+ }
+
+ virtual void sortOverlappingPairs(btDispatcher* dispatcher)
+ {
+ }
+
+
+};
+
+
+#endif //OVERLAPPING_PAIR_CACHE_H
+
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,40 @@
+
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef OVERLAPPING_PAIR_CALLBACK_H
+#define OVERLAPPING_PAIR_CALLBACK_H
+
+class btDispatcher;
+struct btBroadphasePair;
+
+///The btOverlappingPairCallback class is an additional optional broadphase user callback for adding/removing overlapping pairs, similar interface to btOverlappingPairCache.
+class btOverlappingPairCallback
+{
+public:
+ virtual ~btOverlappingPairCallback()
+ {
+
+ }
+
+ virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) = 0;
+
+ virtual void* removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher) = 0;
+
+ virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy0,btDispatcher* dispatcher) = 0;
+
+};
+
+#endif //OVERLAPPING_PAIR_CALLBACK_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btQuantizedBvh.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btQuantizedBvh.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btQuantizedBvh.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btQuantizedBvh.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,473 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef QUANTIZED_BVH_H
+#define QUANTIZED_BVH_H
+
+//#define DEBUG_CHECK_DEQUANTIZATION 1
+#ifdef DEBUG_CHECK_DEQUANTIZATION
+#ifdef __SPU__
+#define printf spu_printf
+#endif //__SPU__
+
+#include <stdio.h>
+#include <stdlib.h>
+#endif //DEBUG_CHECK_DEQUANTIZATION
+
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btAlignedAllocator.h"
+
+
+//http://msdn.microsoft.com/library/default.asp?url=/library/en-us/vclang/html/vclrf__m128.asp
+
+
+//Note: currently we have 16 bytes per quantized node
+#define MAX_SUBTREE_SIZE_IN_BYTES 2048
+
+// 10 gives the potential for 1024 parts, with at most 2^21 (2097152) (minus one
+// actually) triangles each (since the sign bit is reserved
+#define MAX_NUM_PARTS_IN_BITS 10
+
+///btQuantizedBvhNode is a compressed aabb node, 16 bytes.
+///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range).
+ATTRIBUTE_ALIGNED16 (struct) btQuantizedBvhNode
+{
+ BT_DECLARE_ALIGNED_ALLOCATOR();
+
+ //12 bytes
+ unsigned short int m_quantizedAabbMin[3];
+ unsigned short int m_quantizedAabbMax[3];
+ //4 bytes
+ int m_escapeIndexOrTriangleIndex;
+
+ bool isLeafNode() const
+ {
+ //skipindex is negative (internal node), triangleindex >=0 (leafnode)
+ return (m_escapeIndexOrTriangleIndex >= 0);
+ }
+ int getEscapeIndex() const
+ {
+ btAssert(!isLeafNode());
+ return -m_escapeIndexOrTriangleIndex;
+ }
+ int getTriangleIndex() const
+ {
+ btAssert(isLeafNode());
+ // Get only the lower bits where the triangle index is stored
+ return (m_escapeIndexOrTriangleIndex&~((~0)<<(31-MAX_NUM_PARTS_IN_BITS)));
+ }
+ int getPartId() const
+ {
+ btAssert(isLeafNode());
+ // Get only the highest bits where the part index is stored
+ return (m_escapeIndexOrTriangleIndex>>(31-MAX_NUM_PARTS_IN_BITS));
+ }
+}
+;
+
+/// btOptimizedBvhNode contains both internal and leaf node information.
+/// Total node size is 44 bytes / node. You can use the compressed version of 16 bytes.
+ATTRIBUTE_ALIGNED16 (struct) btOptimizedBvhNode
+{
+ BT_DECLARE_ALIGNED_ALLOCATOR();
+
+ //32 bytes
+ btVector3 m_aabbMinOrg;
+ btVector3 m_aabbMaxOrg;
+
+ //4
+ int m_escapeIndex;
+
+ //8
+ //for child nodes
+ int m_subPart;
+ int m_triangleIndex;
+ int m_padding[5];//bad, due to alignment
+
+
+};
+
+
+///btBvhSubtreeInfo provides info to gather a subtree of limited size
+ATTRIBUTE_ALIGNED16(class) btBvhSubtreeInfo
+{
+public:
+ BT_DECLARE_ALIGNED_ALLOCATOR();
+
+ //12 bytes
+ unsigned short int m_quantizedAabbMin[3];
+ unsigned short int m_quantizedAabbMax[3];
+ //4 bytes, points to the root of the subtree
+ int m_rootNodeIndex;
+ //4 bytes
+ int m_subtreeSize;
+ int m_padding[3];
+
+ btBvhSubtreeInfo()
+ {
+ //memset(&m_padding[0], 0, sizeof(m_padding));
+ }
+
+
+ void setAabbFromQuantizeNode(const btQuantizedBvhNode& quantizedNode)
+ {
+ m_quantizedAabbMin[0] = quantizedNode.m_quantizedAabbMin[0];
+ m_quantizedAabbMin[1] = quantizedNode.m_quantizedAabbMin[1];
+ m_quantizedAabbMin[2] = quantizedNode.m_quantizedAabbMin[2];
+ m_quantizedAabbMax[0] = quantizedNode.m_quantizedAabbMax[0];
+ m_quantizedAabbMax[1] = quantizedNode.m_quantizedAabbMax[1];
+ m_quantizedAabbMax[2] = quantizedNode.m_quantizedAabbMax[2];
+ }
+}
+;
+
+
+class btNodeOverlapCallback
+{
+public:
+ virtual ~btNodeOverlapCallback() {};
+
+ virtual void processNode(int subPart, int triangleIndex) = 0;
+};
+
+#include "LinearMath/btAlignedAllocator.h"
+#include "LinearMath/btAlignedObjectArray.h"
+
+
+
+///for code readability:
+typedef btAlignedObjectArray<btOptimizedBvhNode> NodeArray;
+typedef btAlignedObjectArray<btQuantizedBvhNode> QuantizedNodeArray;
+typedef btAlignedObjectArray<btBvhSubtreeInfo> BvhSubtreeInfoArray;
+
+
+///The btQuantizedBvh class stores an AABB tree that can be quickly traversed on CPU and Cell SPU.
+///It is used by the btBvhTriangleMeshShape as midphase, and by the btMultiSapBroadphase.
+///It is recommended to use quantization for better performance and lower memory requirements.
+ATTRIBUTE_ALIGNED16(class) btQuantizedBvh
+{
+public:
+ enum btTraversalMode
+ {
+ TRAVERSAL_STACKLESS = 0,
+ TRAVERSAL_STACKLESS_CACHE_FRIENDLY,
+ TRAVERSAL_RECURSIVE
+ };
+
+protected:
+
+
+ btVector3 m_bvhAabbMin;
+ btVector3 m_bvhAabbMax;
+ btVector3 m_bvhQuantization;
+
+ int m_bulletVersion; //for serialization versioning. It could also be used to detect endianess.
+
+ int m_curNodeIndex;
+ //quantization data
+ bool m_useQuantization;
+
+
+
+ NodeArray m_leafNodes;
+ NodeArray m_contiguousNodes;
+ QuantizedNodeArray m_quantizedLeafNodes;
+ QuantizedNodeArray m_quantizedContiguousNodes;
+
+ btTraversalMode m_traversalMode;
+ BvhSubtreeInfoArray m_SubtreeHeaders;
+
+ //This is only used for serialization so we don't have to add serialization directly to btAlignedObjectArray
+ int m_subtreeHeaderCount;
+
+
+
+
+
+ ///two versions, one for quantized and normal nodes. This allows code-reuse while maintaining readability (no template/macro!)
+ ///this might be refactored into a virtual, it is usually not calculated at run-time
+ void setInternalNodeAabbMin(int nodeIndex, const btVector3& aabbMin)
+ {
+ if (m_useQuantization)
+ {
+ quantize(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0] ,aabbMin,0);
+ } else
+ {
+ m_contiguousNodes[nodeIndex].m_aabbMinOrg = aabbMin;
+
+ }
+ }
+ void setInternalNodeAabbMax(int nodeIndex,const btVector3& aabbMax)
+ {
+ if (m_useQuantization)
+ {
+ quantize(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0],aabbMax,1);
+ } else
+ {
+ m_contiguousNodes[nodeIndex].m_aabbMaxOrg = aabbMax;
+ }
+ }
+
+ btVector3 getAabbMin(int nodeIndex) const
+ {
+ if (m_useQuantization)
+ {
+ return unQuantize(&m_quantizedLeafNodes[nodeIndex].m_quantizedAabbMin[0]);
+ }
+ //non-quantized
+ return m_leafNodes[nodeIndex].m_aabbMinOrg;
+
+ }
+ btVector3 getAabbMax(int nodeIndex) const
+ {
+ if (m_useQuantization)
+ {
+ return unQuantize(&m_quantizedLeafNodes[nodeIndex].m_quantizedAabbMax[0]);
+ }
+ //non-quantized
+ return m_leafNodes[nodeIndex].m_aabbMaxOrg;
+
+ }
+
+
+ void setInternalNodeEscapeIndex(int nodeIndex, int escapeIndex)
+ {
+ if (m_useQuantization)
+ {
+ m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex = -escapeIndex;
+ }
+ else
+ {
+ m_contiguousNodes[nodeIndex].m_escapeIndex = escapeIndex;
+ }
+
+ }
+
+ void mergeInternalNodeAabb(int nodeIndex,const btVector3& newAabbMin,const btVector3& newAabbMax)
+ {
+ if (m_useQuantization)
+ {
+ unsigned short int quantizedAabbMin[3];
+ unsigned short int quantizedAabbMax[3];
+ quantize(quantizedAabbMin,newAabbMin,0);
+ quantize(quantizedAabbMax,newAabbMax,1);
+ for (int i=0;i<3;i++)
+ {
+ if (m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[i] > quantizedAabbMin[i])
+ m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[i] = quantizedAabbMin[i];
+
+ if (m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[i] < quantizedAabbMax[i])
+ m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[i] = quantizedAabbMax[i];
+
+ }
+ } else
+ {
+ //non-quantized
+ m_contiguousNodes[nodeIndex].m_aabbMinOrg.setMin(newAabbMin);
+ m_contiguousNodes[nodeIndex].m_aabbMaxOrg.setMax(newAabbMax);
+ }
+ }
+
+ void swapLeafNodes(int firstIndex,int secondIndex);
+
+ void assignInternalNodeFromLeafNode(int internalNode,int leafNodeIndex);
+
+protected:
+
+
+
+ void buildTree (int startIndex,int endIndex);
+
+ int calcSplittingAxis(int startIndex,int endIndex);
+
+ int sortAndCalcSplittingIndex(int startIndex,int endIndex,int splitAxis);
+
+ void walkStacklessTree(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const;
+
+ void walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const;
+ void walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,int startNodeIndex,int endNodeIndex) const;
+ void walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const;
+
+ ///tree traversal designed for small-memory processors like PS3 SPU
+ void walkStacklessQuantizedTreeCacheFriendly(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const;
+
+ ///use the 16-byte stackless 'skipindex' node tree to do a recursive traversal
+ void walkRecursiveQuantizedTreeAgainstQueryAabb(const btQuantizedBvhNode* currentNode,btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const;
+
+ ///use the 16-byte stackless 'skipindex' node tree to do a recursive traversal
+ void walkRecursiveQuantizedTreeAgainstQuantizedTree(const btQuantizedBvhNode* treeNodeA,const btQuantizedBvhNode* treeNodeB,btNodeOverlapCallback* nodeCallback) const;
+
+
+
+
+ void updateSubtreeHeaders(int leftChildNodexIndex,int rightChildNodexIndex);
+
+public:
+
+ BT_DECLARE_ALIGNED_ALLOCATOR();
+
+ btQuantizedBvh();
+
+ virtual ~btQuantizedBvh();
+
+
+ ///***************************************** expert/internal use only *************************
+ void setQuantizationValues(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax,btScalar quantizationMargin=btScalar(1.0));
+ QuantizedNodeArray& getLeafNodeArray() { return m_quantizedLeafNodes; }
+ ///buildInternal is expert use only: assumes that setQuantizationValues and LeafNodeArray are initialized
+ void buildInternal();
+ ///***************************************** expert/internal use only *************************
+
+ void reportAabbOverlappingNodex(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const;
+ void reportRayOverlappingNodex (btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget) const;
+ void reportBoxCastOverlappingNodex(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin,const btVector3& aabbMax) const;
+
+ SIMD_FORCE_INLINE void quantize(unsigned short* out, const btVector3& point,int isMax) const
+ {
+
+ btAssert(m_useQuantization);
+
+ btAssert(point.getX() <= m_bvhAabbMax.getX());
+ btAssert(point.getY() <= m_bvhAabbMax.getY());
+ btAssert(point.getZ() <= m_bvhAabbMax.getZ());
+
+ btAssert(point.getX() >= m_bvhAabbMin.getX());
+ btAssert(point.getY() >= m_bvhAabbMin.getY());
+ btAssert(point.getZ() >= m_bvhAabbMin.getZ());
+
+ btVector3 v = (point - m_bvhAabbMin) * m_bvhQuantization;
+ ///Make sure rounding is done in a way that unQuantize(quantizeWithClamp(...)) is conservative
+ ///end-points always set the first bit, so that they are sorted properly (so that neighbouring AABBs overlap properly)
+ ///@todo: double-check this
+ if (isMax)
+ {
+ out[0] = (unsigned short) (((unsigned short)(v.getX()+btScalar(1.)) | 1));
+ out[1] = (unsigned short) (((unsigned short)(v.getY()+btScalar(1.)) | 1));
+ out[2] = (unsigned short) (((unsigned short)(v.getZ()+btScalar(1.)) | 1));
+ } else
+ {
+ out[0] = (unsigned short) (((unsigned short)(v.getX()) & 0xfffe));
+ out[1] = (unsigned short) (((unsigned short)(v.getY()) & 0xfffe));
+ out[2] = (unsigned short) (((unsigned short)(v.getZ()) & 0xfffe));
+ }
+
+
+#ifdef DEBUG_CHECK_DEQUANTIZATION
+ btVector3 newPoint = unQuantize(out);
+ if (isMax)
+ {
+ if (newPoint.getX() < point.getX())
+ {
+ printf("unconservative X, diffX = %f, oldX=%f,newX=%f\n",newPoint.getX()-point.getX(), newPoint.getX(),point.getX());
+ }
+ if (newPoint.getY() < point.getY())
+ {
+ printf("unconservative Y, diffY = %f, oldY=%f,newY=%f\n",newPoint.getY()-point.getY(), newPoint.getY(),point.getY());
+ }
+ if (newPoint.getZ() < point.getZ())
+ {
+
+ printf("unconservative Z, diffZ = %f, oldZ=%f,newZ=%f\n",newPoint.getZ()-point.getZ(), newPoint.getZ(),point.getZ());
+ }
+ } else
+ {
+ if (newPoint.getX() > point.getX())
+ {
+ printf("unconservative X, diffX = %f, oldX=%f,newX=%f\n",newPoint.getX()-point.getX(), newPoint.getX(),point.getX());
+ }
+ if (newPoint.getY() > point.getY())
+ {
+ printf("unconservative Y, diffY = %f, oldY=%f,newY=%f\n",newPoint.getY()-point.getY(), newPoint.getY(),point.getY());
+ }
+ if (newPoint.getZ() > point.getZ())
+ {
+ printf("unconservative Z, diffZ = %f, oldZ=%f,newZ=%f\n",newPoint.getZ()-point.getZ(), newPoint.getZ(),point.getZ());
+ }
+ }
+#endif //DEBUG_CHECK_DEQUANTIZATION
+
+ }
+
+
+ SIMD_FORCE_INLINE void quantizeWithClamp(unsigned short* out, const btVector3& point2,int isMax) const
+ {
+
+ btAssert(m_useQuantization);
+
+ btVector3 clampedPoint(point2);
+ clampedPoint.setMax(m_bvhAabbMin);
+ clampedPoint.setMin(m_bvhAabbMax);
+
+ quantize(out,clampedPoint,isMax);
+
+ }
+
+ SIMD_FORCE_INLINE btVector3 unQuantize(const unsigned short* vecIn) const
+ {
+ btVector3 vecOut;
+ vecOut.setValue(
+ (btScalar)(vecIn[0]) / (m_bvhQuantization.getX()),
+ (btScalar)(vecIn[1]) / (m_bvhQuantization.getY()),
+ (btScalar)(vecIn[2]) / (m_bvhQuantization.getZ()));
+ vecOut += m_bvhAabbMin;
+ return vecOut;
+ }
+
+ ///setTraversalMode let's you choose between stackless, recursive or stackless cache friendly tree traversal. Note this is only implemented for quantized trees.
+ void setTraversalMode(btTraversalMode traversalMode)
+ {
+ m_traversalMode = traversalMode;
+ }
+
+
+ SIMD_FORCE_INLINE QuantizedNodeArray& getQuantizedNodeArray()
+ {
+ return m_quantizedContiguousNodes;
+ }
+
+
+ SIMD_FORCE_INLINE BvhSubtreeInfoArray& getSubtreeInfoArray()
+ {
+ return m_SubtreeHeaders;
+ }
+
+
+ /////Calculate space needed to store BVH for serialization
+ unsigned calculateSerializeBufferSize();
+
+ /// Data buffer MUST be 16 byte aligned
+ virtual bool serialize(void *o_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian);
+
+ ///deSerializeInPlace loads and initializes a BVH from a buffer in memory 'in place'
+ static btQuantizedBvh *deSerializeInPlace(void *i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian);
+
+ static unsigned int getAlignmentSerializationPadding();
+
+ SIMD_FORCE_INLINE bool isQuantized()
+ {
+ return m_useQuantization;
+ }
+
+private:
+ // Special "copy" constructor that allows for in-place deserialization
+ // Prevents btVector3's default constructor from being called, but doesn't inialize much else
+ // ownsMemory should most likely be false if deserializing, and if you are not, don't call this (it also changes the function signature, which we need)
+ btQuantizedBvh(btQuantizedBvh &other, bool ownsMemory);
+
+}
+;
+
+
+#endif //QUANTIZED_BVH_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,170 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef SIMPLE_BROADPHASE_H
+#define SIMPLE_BROADPHASE_H
+
+
+#include "btOverlappingPairCache.h"
+
+
+struct btSimpleBroadphaseProxy : public btBroadphaseProxy
+{
+ int m_nextFree;
+
+// int m_handleId;
+
+
+ btSimpleBroadphaseProxy() {};
+
+ btSimpleBroadphaseProxy(const btVector3& minpt,const btVector3& maxpt,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,void* multiSapProxy)
+ :btBroadphaseProxy(minpt,maxpt,userPtr,collisionFilterGroup,collisionFilterMask,multiSapProxy)
+ {
+ (void)shapeType;
+ }
+
+
+ SIMD_FORCE_INLINE void SetNextFree(int next) {m_nextFree = next;}
+ SIMD_FORCE_INLINE int GetNextFree() const {return m_nextFree;}
+
+
+
+
+};
+
+///The SimpleBroadphase is just a unit-test for btAxisSweep3, bt32BitAxisSweep3, or btDbvtBroadphase, so use those classes instead.
+///It is a brute force aabb culling broadphase based on O(n^2) aabb checks
+class btSimpleBroadphase : public btBroadphaseInterface
+{
+
+protected:
+
+ int m_numHandles; // number of active handles
+ int m_maxHandles; // max number of handles
+ int m_LastHandleIndex;
+
+ btSimpleBroadphaseProxy* m_pHandles; // handles pool
+
+ void* m_pHandlesRawPtr;
+ int m_firstFreeHandle; // free handles list
+
+ int allocHandle()
+ {
+ btAssert(m_numHandles < m_maxHandles);
+ int freeHandle = m_firstFreeHandle;
+ m_firstFreeHandle = m_pHandles[freeHandle].GetNextFree();
+ m_numHandles++;
+ if(freeHandle > m_LastHandleIndex)
+ {
+ m_LastHandleIndex = freeHandle;
+ }
+ return freeHandle;
+ }
+
+ void freeHandle(btSimpleBroadphaseProxy* proxy)
+ {
+ int handle = int(proxy-m_pHandles);
+ btAssert(handle >= 0 && handle < m_maxHandles);
+ if(handle == m_LastHandleIndex)
+ {
+ m_LastHandleIndex--;
+ }
+ proxy->SetNextFree(m_firstFreeHandle);
+ m_firstFreeHandle = handle;
+
+ proxy->m_clientObject = 0;
+
+ m_numHandles--;
+ }
+
+ btOverlappingPairCache* m_pairCache;
+ bool m_ownsPairCache;
+
+ int m_invalidPair;
+
+
+
+ inline btSimpleBroadphaseProxy* getSimpleProxyFromProxy(btBroadphaseProxy* proxy)
+ {
+ btSimpleBroadphaseProxy* proxy0 = static_cast<btSimpleBroadphaseProxy*>(proxy);
+ return proxy0;
+ }
+
+ inline const btSimpleBroadphaseProxy* getSimpleProxyFromProxy(btBroadphaseProxy* proxy) const
+ {
+ const btSimpleBroadphaseProxy* proxy0 = static_cast<const btSimpleBroadphaseProxy*>(proxy);
+ return proxy0;
+ }
+
+ ///reset broadphase internal structures, to ensure determinism/reproducability
+ virtual void resetPool(btDispatcher* dispatcher);
+
+
+ void validate();
+
+protected:
+
+
+
+
+public:
+ btSimpleBroadphase(int maxProxies=16384,btOverlappingPairCache* overlappingPairCache=0);
+ virtual ~btSimpleBroadphase();
+
+
+ static bool aabbOverlap(btSimpleBroadphaseProxy* proxy0,btSimpleBroadphaseProxy* proxy1);
+
+
+ virtual btBroadphaseProxy* createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* multiSapProxy);
+
+ virtual void calculateOverlappingPairs(btDispatcher* dispatcher);
+
+ virtual void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
+ virtual void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher);
+ virtual void getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const;
+
+ virtual void rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin=btVector3(0,0,0),const btVector3& aabbMax=btVector3(0,0,0));
+
+ btOverlappingPairCache* getOverlappingPairCache()
+ {
+ return m_pairCache;
+ }
+ const btOverlappingPairCache* getOverlappingPairCache() const
+ {
+ return m_pairCache;
+ }
+
+ bool testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
+
+
+ ///getAabb returns the axis aligned bounding box in the 'global' coordinate frame
+ ///will add some transform later
+ virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const
+ {
+ aabbMin.setValue(-BT_LARGE_FLOAT,-BT_LARGE_FLOAT,-BT_LARGE_FLOAT);
+ aabbMax.setValue(BT_LARGE_FLOAT,BT_LARGE_FLOAT,BT_LARGE_FLOAT);
+ }
+
+ virtual void printStats()
+ {
+// printf("btSimpleBroadphase.h\n");
+// printf("numHandles = %d, maxHandles = %d\n",m_numHandles,m_maxHandles);
+ }
+};
+
+
+
+#endif //SIMPLE_BROADPHASE_H
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/SphereTriangleDetector.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/SphereTriangleDetector.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/SphereTriangleDetector.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/SphereTriangleDetector.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,49 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef SPHERE_TRIANGLE_DETECTOR_H
+#define SPHERE_TRIANGLE_DETECTOR_H
+
+#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
+
+
+
+class btSphereShape;
+class btTriangleShape;
+
+
+
+/// sphere-triangle to match the btDiscreteCollisionDetectorInterface
+struct SphereTriangleDetector : public btDiscreteCollisionDetectorInterface
+{
+ virtual void getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false);
+
+ SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle, btScalar contactBreakingThreshold);
+
+ virtual ~SphereTriangleDetector() {};
+
+private:
+
+ bool collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact, btScalar contactBreakingThreshold);
+ bool pointInTriangle(const btVector3 vertices[], const btVector3 &normal, btVector3 *p );
+ bool facecontains(const btVector3 &p,const btVector3* vertices,btVector3& normal);
+
+ btSphereShape* m_sphere;
+ btTriangleShape* m_triangle;
+ btScalar m_contactBreakingThreshold;
+
+};
+#endif //SPHERE_TRIANGLE_DETECTOR_H
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,36 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef __BT_ACTIVATING_COLLISION_ALGORITHM_H
+#define __BT_ACTIVATING_COLLISION_ALGORITHM_H
+
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+
+///This class is not enabled yet (work-in-progress) to more aggressively activate objects.
+class btActivatingCollisionAlgorithm : public btCollisionAlgorithm
+{
+// btCollisionObject* m_colObj0;
+// btCollisionObject* m_colObj1;
+
+public:
+
+ btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci);
+
+ btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* colObj0,btCollisionObject* colObj1);
+
+ virtual ~btActivatingCollisionAlgorithm();
+
+};
+#endif //__BT_ACTIVATING_COLLISION_ALGORITHM_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,66 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BOX_2D_BOX_2D__COLLISION_ALGORITHM_H
+#define BOX_2D_BOX_2D__COLLISION_ALGORITHM_H
+
+#include "BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
+#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
+
+class btPersistentManifold;
+
+///box-box collision detection
+class btBox2dBox2dCollisionAlgorithm : public btActivatingCollisionAlgorithm
+{
+ bool m_ownManifold;
+ btPersistentManifold* m_manifoldPtr;
+
+public:
+ btBox2dBox2dCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
+ : btActivatingCollisionAlgorithm(ci) {}
+
+ virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+ virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+ btBox2dBox2dCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1);
+
+ virtual ~btBox2dBox2dCollisionAlgorithm();
+
+ virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
+ {
+ if (m_manifoldPtr && m_ownManifold)
+ {
+ manifoldArray.push_back(m_manifoldPtr);
+ }
+ }
+
+
+ struct CreateFunc :public btCollisionAlgorithmCreateFunc
+ {
+ virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+ {
+ int bbsize = sizeof(btBox2dBox2dCollisionAlgorithm);
+ void* ptr = ci.m_dispatcher1->allocateCollisionAlgorithm(bbsize);
+ return new(ptr) btBox2dBox2dCollisionAlgorithm(0,ci,body0,body1);
+ }
+ };
+
+};
+
+#endif //BOX_2D_BOX_2D__COLLISION_ALGORITHM_H
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,66 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BOX_BOX__COLLISION_ALGORITHM_H
+#define BOX_BOX__COLLISION_ALGORITHM_H
+
+#include "btActivatingCollisionAlgorithm.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
+#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
+
+class btPersistentManifold;
+
+///box-box collision detection
+class btBoxBoxCollisionAlgorithm : public btActivatingCollisionAlgorithm
+{
+ bool m_ownManifold;
+ btPersistentManifold* m_manifoldPtr;
+
+public:
+ btBoxBoxCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
+ : btActivatingCollisionAlgorithm(ci) {}
+
+ virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+ virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+ btBoxBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1);
+
+ virtual ~btBoxBoxCollisionAlgorithm();
+
+ virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
+ {
+ if (m_manifoldPtr && m_ownManifold)
+ {
+ manifoldArray.push_back(m_manifoldPtr);
+ }
+ }
+
+
+ struct CreateFunc :public btCollisionAlgorithmCreateFunc
+ {
+ virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+ {
+ int bbsize = sizeof(btBoxBoxCollisionAlgorithm);
+ void* ptr = ci.m_dispatcher1->allocateCollisionAlgorithm(bbsize);
+ return new(ptr) btBoxBoxCollisionAlgorithm(0,ci,body0,body1);
+ }
+ };
+
+};
+
+#endif //BOX_BOX__COLLISION_ALGORITHM_H
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btBoxBoxDetector.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btBoxBoxDetector.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btBoxBoxDetector.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btBoxBoxDetector.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,44 @@
+/*
+ * Box-Box collision detection re-distributed under the ZLib license with permission from Russell L. Smith
+ * Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.
+ * All rights reserved. Email: russ at q12.org Web: www.q12.org
+
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+#ifndef BOX_BOX_DETECTOR_H
+#define BOX_BOX_DETECTOR_H
+
+
+class btBoxShape;
+#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
+
+
+/// btBoxBoxDetector wraps the ODE box-box collision detector
+/// re-distributed under the Zlib license with permission from Russell L. Smith
+struct btBoxBoxDetector : public btDiscreteCollisionDetectorInterface
+{
+ btBoxShape* m_box1;
+ btBoxShape* m_box2;
+
+public:
+
+ btBoxBoxDetector(btBoxShape* box1,btBoxShape* box2);
+
+ virtual ~btBoxBoxDetector() {};
+
+ virtual void getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false);
+
+};
+
+#endif //BT_BOX_BOX_DETECTOR_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btCollisionConfiguration.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btCollisionConfiguration.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btCollisionConfiguration.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btCollisionConfiguration.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,47 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_COLLISION_CONFIGURATION
+#define BT_COLLISION_CONFIGURATION
+struct btCollisionAlgorithmCreateFunc;
+
+class btStackAlloc;
+class btPoolAllocator;
+
+///btCollisionConfiguration allows to configure Bullet collision detection
+///stack allocator size, default collision algorithms and persistent manifold pool size
+///@todo: describe the meaning
+class btCollisionConfiguration
+{
+
+public:
+
+ virtual ~btCollisionConfiguration()
+ {
+ }
+
+ ///memory pools
+ virtual btPoolAllocator* getPersistentManifoldPool() = 0;
+
+ virtual btPoolAllocator* getCollisionAlgorithmPool() = 0;
+
+ virtual btStackAlloc* getStackAllocator() = 0;
+
+ virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1) =0;
+
+};
+
+#endif //BT_COLLISION_CONFIGURATION
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,45 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef COLLISION_CREATE_FUNC
+#define COLLISION_CREATE_FUNC
+
+#include "LinearMath/btAlignedObjectArray.h"
+class btCollisionAlgorithm;
+class btCollisionObject;
+
+struct btCollisionAlgorithmConstructionInfo;
+
+///Used by the btCollisionDispatcher to register and create instances for btCollisionAlgorithm
+struct btCollisionAlgorithmCreateFunc
+{
+ bool m_swapped;
+
+ btCollisionAlgorithmCreateFunc()
+ :m_swapped(false)
+ {
+ }
+ virtual ~btCollisionAlgorithmCreateFunc(){};
+
+ virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& , btCollisionObject* body0,btCollisionObject* body1)
+ {
+
+ (void)body0;
+ (void)body1;
+ return 0;
+ }
+};
+#endif //COLLISION_CREATE_FUNC
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btCollisionDispatcher.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btCollisionDispatcher.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btCollisionDispatcher.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btCollisionDispatcher.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,147 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef COLLISION__DISPATCHER_H
+#define COLLISION__DISPATCHER_H
+
+#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+
+#include "BulletCollision/CollisionDispatch/btManifoldResult.h"
+
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "LinearMath/btAlignedObjectArray.h"
+
+class btIDebugDraw;
+class btOverlappingPairCache;
+class btPoolAllocator;
+class btCollisionConfiguration;
+
+#include "btCollisionCreateFunc.h"
+
+#define USE_DISPATCH_REGISTRY_ARRAY 1
+
+class btCollisionDispatcher;
+///user can override this nearcallback for collision filtering and more finegrained control over collision detection
+typedef void (*btNearCallback)(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo);
+
+
+///btCollisionDispatcher supports algorithms that handle ConvexConvex and ConvexConcave collision pairs.
+///Time of Impact, Closest Points and Penetration Depth.
+class btCollisionDispatcher : public btDispatcher
+{
+ int m_count;
+
+ btAlignedObjectArray<btPersistentManifold*> m_manifoldsPtr;
+
+ bool m_useIslands;
+
+ bool m_staticWarningReported;
+
+ btManifoldResult m_defaultManifoldResult;
+
+ btNearCallback m_nearCallback;
+
+ btPoolAllocator* m_collisionAlgorithmPoolAllocator;
+
+ btPoolAllocator* m_persistentManifoldPoolAllocator;
+
+ btCollisionAlgorithmCreateFunc* m_doubleDispatch[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES];
+
+
+ btCollisionConfiguration* m_collisionConfiguration;
+
+
+public:
+
+ ///registerCollisionCreateFunc allows registration of custom/alternative collision create functions
+ void registerCollisionCreateFunc(int proxyType0,int proxyType1, btCollisionAlgorithmCreateFunc* createFunc);
+
+ int getNumManifolds() const
+ {
+ return int( m_manifoldsPtr.size());
+ }
+
+ btPersistentManifold** getInternalManifoldPointer()
+ {
+ return &m_manifoldsPtr[0];
+ }
+
+ btPersistentManifold* getManifoldByIndexInternal(int index)
+ {
+ return m_manifoldsPtr[index];
+ }
+
+ const btPersistentManifold* getManifoldByIndexInternal(int index) const
+ {
+ return m_manifoldsPtr[index];
+ }
+
+ btCollisionDispatcher (btCollisionConfiguration* collisionConfiguration);
+
+ virtual ~btCollisionDispatcher();
+
+ virtual btPersistentManifold* getNewManifold(void* b0,void* b1);
+
+ virtual void releaseManifold(btPersistentManifold* manifold);
+
+
+ virtual void clearManifold(btPersistentManifold* manifold);
+
+
+ btCollisionAlgorithm* findAlgorithm(btCollisionObject* body0,btCollisionObject* body1,btPersistentManifold* sharedManifold = 0);
+
+ virtual bool needsCollision(btCollisionObject* body0,btCollisionObject* body1);
+
+ virtual bool needsResponse(btCollisionObject* body0,btCollisionObject* body1);
+
+ virtual void dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,const btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher) ;
+
+ void setNearCallback(btNearCallback nearCallback)
+ {
+ m_nearCallback = nearCallback;
+ }
+
+ btNearCallback getNearCallback() const
+ {
+ return m_nearCallback;
+ }
+
+ //by default, Bullet will use this near callback
+ static void defaultNearCallback(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo);
+
+ virtual void* allocateCollisionAlgorithm(int size);
+
+ virtual void freeCollisionAlgorithm(void* ptr);
+
+ btCollisionConfiguration* getCollisionConfiguration()
+ {
+ return m_collisionConfiguration;
+ }
+
+ const btCollisionConfiguration* getCollisionConfiguration() const
+ {
+ return m_collisionConfiguration;
+ }
+
+ void setCollisionConfiguration(btCollisionConfiguration* config)
+ {
+ m_collisionConfiguration = config;
+ }
+
+};
+
+#endif //COLLISION__DISPATCHER_H
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btCollisionObject.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btCollisionObject.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btCollisionObject.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btCollisionObject.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,421 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef COLLISION_OBJECT_H
+#define COLLISION_OBJECT_H
+
+#include "LinearMath/btTransform.h"
+
+//island management, m_activationState1
+#define ACTIVE_TAG 1
+#define ISLAND_SLEEPING 2
+#define WANTS_DEACTIVATION 3
+#define DISABLE_DEACTIVATION 4
+#define DISABLE_SIMULATION 5
+
+struct btBroadphaseProxy;
+class btCollisionShape;
+#include "LinearMath/btMotionState.h"
+#include "LinearMath/btAlignedAllocator.h"
+#include "LinearMath/btAlignedObjectArray.h"
+
+
+typedef btAlignedObjectArray<class btCollisionObject*> btCollisionObjectArray;
+
+
+/// btCollisionObject can be used to manage collision detection objects.
+/// btCollisionObject maintains all information that is needed for a collision detection: Shape, Transform and AABB proxy.
+/// They can be added to the btCollisionWorld.
+ATTRIBUTE_ALIGNED16(class) btCollisionObject
+{
+
+protected:
+
+ btTransform m_worldTransform;
+
+ ///m_interpolationWorldTransform is used for CCD and interpolation
+ ///it can be either previous or future (predicted) transform
+ btTransform m_interpolationWorldTransform;
+ //those two are experimental: just added for bullet time effect, so you can still apply impulses (directly modifying velocities)
+ //without destroying the continuous interpolated motion (which uses this interpolation velocities)
+ btVector3 m_interpolationLinearVelocity;
+ btVector3 m_interpolationAngularVelocity;
+
+ btVector3 m_anisotropicFriction;
+ bool m_hasAnisotropicFriction;
+ btScalar m_contactProcessingThreshold;
+
+ btBroadphaseProxy* m_broadphaseHandle;
+ btCollisionShape* m_collisionShape;
+
+ ///m_rootCollisionShape is temporarily used to store the original collision shape
+ ///The m_collisionShape might be temporarily replaced by a child collision shape during collision detection purposes
+ ///If it is NULL, the m_collisionShape is not temporarily replaced.
+ btCollisionShape* m_rootCollisionShape;
+
+ int m_collisionFlags;
+
+ int m_islandTag1;
+ int m_companionId;
+
+ int m_activationState1;
+ btScalar m_deactivationTime;
+
+ btScalar m_friction;
+ btScalar m_restitution;
+
+ ///users can point to their objects, m_userPointer is not used by Bullet, see setUserPointer/getUserPointer
+ void* m_userObjectPointer;
+
+ ///m_internalType is reserved to distinguish Bullet's btCollisionObject, btRigidBody, btSoftBody, btGhostObject etc.
+ ///do not assign your own m_internalType unless you write a new dynamics object class.
+ int m_internalType;
+
+ ///time of impact calculation
+ btScalar m_hitFraction;
+
+ ///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm::
+ btScalar m_ccdSweptSphereRadius;
+
+ /// Don't do continuous collision detection if the motion (in one step) is less then m_ccdMotionThreshold
+ btScalar m_ccdMotionThreshold;
+
+ /// If some object should have elaborate collision filtering by sub-classes
+ bool m_checkCollideWith;
+
+ char m_pad[7];
+
+ virtual bool checkCollideWithOverride(btCollisionObject* /* co */)
+ {
+ return true;
+ }
+
+public:
+
+ BT_DECLARE_ALIGNED_ALLOCATOR();
+
+ enum CollisionFlags
+ {
+ CF_STATIC_OBJECT= 1,
+ CF_KINEMATIC_OBJECT= 2,
+ CF_NO_CONTACT_RESPONSE = 4,
+ CF_CUSTOM_MATERIAL_CALLBACK = 8,//this allows per-triangle material (friction/restitution)
+ CF_CHARACTER_OBJECT = 16
+ };
+
+ enum CollisionObjectTypes
+ {
+ CO_COLLISION_OBJECT =1,
+ CO_RIGID_BODY,
+ ///CO_GHOST_OBJECT keeps track of all objects overlapping its AABB and that pass its collision filter
+ ///It is useful for collision sensors, explosion objects, character controller etc.
+ CO_GHOST_OBJECT,
+ CO_SOFT_BODY,
+ CO_HF_FLUID
+ };
+
+ SIMD_FORCE_INLINE bool mergesSimulationIslands() const
+ {
+ ///static objects, kinematic and object without contact response don't merge islands
+ return ((m_collisionFlags & (CF_STATIC_OBJECT | CF_KINEMATIC_OBJECT | CF_NO_CONTACT_RESPONSE) )==0);
+ }
+
+ const btVector3& getAnisotropicFriction() const
+ {
+ return m_anisotropicFriction;
+ }
+ void setAnisotropicFriction(const btVector3& anisotropicFriction)
+ {
+ m_anisotropicFriction = anisotropicFriction;
+ m_hasAnisotropicFriction = (anisotropicFriction[0]!=1.f) || (anisotropicFriction[1]!=1.f) || (anisotropicFriction[2]!=1.f);
+ }
+ bool hasAnisotropicFriction() const
+ {
+ return m_hasAnisotropicFriction;
+ }
+
+ ///the constraint solver can discard solving contacts, if the distance is above this threshold. 0 by default.
+ ///Note that using contacts with positive distance can improve stability. It increases, however, the chance of colliding with degerate contacts, such as 'interior' triangle edges
+ void setContactProcessingThreshold( btScalar contactProcessingThreshold)
+ {
+ m_contactProcessingThreshold = contactProcessingThreshold;
+ }
+ btScalar getContactProcessingThreshold() const
+ {
+ return m_contactProcessingThreshold;
+ }
+
+ SIMD_FORCE_INLINE bool isStaticObject() const {
+ return (m_collisionFlags & CF_STATIC_OBJECT) != 0;
+ }
+
+ SIMD_FORCE_INLINE bool isKinematicObject() const
+ {
+ return (m_collisionFlags & CF_KINEMATIC_OBJECT) != 0;
+ }
+
+ SIMD_FORCE_INLINE bool isStaticOrKinematicObject() const
+ {
+ return (m_collisionFlags & (CF_KINEMATIC_OBJECT | CF_STATIC_OBJECT)) != 0 ;
+ }
+
+ SIMD_FORCE_INLINE bool hasContactResponse() const {
+ return (m_collisionFlags & CF_NO_CONTACT_RESPONSE)==0;
+ }
+
+
+ btCollisionObject();
+
+ virtual ~btCollisionObject();
+
+ virtual void setCollisionShape(btCollisionShape* collisionShape)
+ {
+ m_collisionShape = collisionShape;
+ m_rootCollisionShape = collisionShape;
+ }
+
+ SIMD_FORCE_INLINE const btCollisionShape* getCollisionShape() const
+ {
+ return m_collisionShape;
+ }
+
+ SIMD_FORCE_INLINE btCollisionShape* getCollisionShape()
+ {
+ return m_collisionShape;
+ }
+
+ SIMD_FORCE_INLINE const btCollisionShape* getRootCollisionShape() const
+ {
+ return m_rootCollisionShape;
+ }
+
+ SIMD_FORCE_INLINE btCollisionShape* getRootCollisionShape()
+ {
+ return m_rootCollisionShape;
+ }
+
+ ///Avoid using this internal API call
+ ///internalSetTemporaryCollisionShape is used to temporary replace the actual collision shape by a child collision shape.
+ void internalSetTemporaryCollisionShape(btCollisionShape* collisionShape)
+ {
+ m_collisionShape = collisionShape;
+ }
+
+ SIMD_FORCE_INLINE int getActivationState() const { return m_activationState1;}
+
+ void setActivationState(int newState);
+
+ void setDeactivationTime(btScalar time)
+ {
+ m_deactivationTime = time;
+ }
+ btScalar getDeactivationTime() const
+ {
+ return m_deactivationTime;
+ }
+
+ void forceActivationState(int newState);
+
+ void activate(bool forceActivation = false);
+
+ SIMD_FORCE_INLINE bool isActive() const
+ {
+ return ((getActivationState() != ISLAND_SLEEPING) && (getActivationState() != DISABLE_SIMULATION));
+ }
+
+ void setRestitution(btScalar rest)
+ {
+ m_restitution = rest;
+ }
+ btScalar getRestitution() const
+ {
+ return m_restitution;
+ }
+ void setFriction(btScalar frict)
+ {
+ m_friction = frict;
+ }
+ btScalar getFriction() const
+ {
+ return m_friction;
+ }
+
+ ///reserved for Bullet internal usage
+ int getInternalType() const
+ {
+ return m_internalType;
+ }
+
+ btTransform& getWorldTransform()
+ {
+ return m_worldTransform;
+ }
+
+ const btTransform& getWorldTransform() const
+ {
+ return m_worldTransform;
+ }
+
+ void setWorldTransform(const btTransform& worldTrans)
+ {
+ m_worldTransform = worldTrans;
+ }
+
+
+ SIMD_FORCE_INLINE btBroadphaseProxy* getBroadphaseHandle()
+ {
+ return m_broadphaseHandle;
+ }
+
+ SIMD_FORCE_INLINE const btBroadphaseProxy* getBroadphaseHandle() const
+ {
+ return m_broadphaseHandle;
+ }
+
+ void setBroadphaseHandle(btBroadphaseProxy* handle)
+ {
+ m_broadphaseHandle = handle;
+ }
+
+
+ const btTransform& getInterpolationWorldTransform() const
+ {
+ return m_interpolationWorldTransform;
+ }
+
+ btTransform& getInterpolationWorldTransform()
+ {
+ return m_interpolationWorldTransform;
+ }
+
+ void setInterpolationWorldTransform(const btTransform& trans)
+ {
+ m_interpolationWorldTransform = trans;
+ }
+
+ void setInterpolationLinearVelocity(const btVector3& linvel)
+ {
+ m_interpolationLinearVelocity = linvel;
+ }
+
+ void setInterpolationAngularVelocity(const btVector3& angvel)
+ {
+ m_interpolationAngularVelocity = angvel;
+ }
+
+ const btVector3& getInterpolationLinearVelocity() const
+ {
+ return m_interpolationLinearVelocity;
+ }
+
+ const btVector3& getInterpolationAngularVelocity() const
+ {
+ return m_interpolationAngularVelocity;
+ }
+
+ SIMD_FORCE_INLINE int getIslandTag() const
+ {
+ return m_islandTag1;
+ }
+
+ void setIslandTag(int tag)
+ {
+ m_islandTag1 = tag;
+ }
+
+ SIMD_FORCE_INLINE int getCompanionId() const
+ {
+ return m_companionId;
+ }
+
+ void setCompanionId(int id)
+ {
+ m_companionId = id;
+ }
+
+ SIMD_FORCE_INLINE btScalar getHitFraction() const
+ {
+ return m_hitFraction;
+ }
+
+ void setHitFraction(btScalar hitFraction)
+ {
+ m_hitFraction = hitFraction;
+ }
+
+
+ SIMD_FORCE_INLINE int getCollisionFlags() const
+ {
+ return m_collisionFlags;
+ }
+
+ void setCollisionFlags(int flags)
+ {
+ m_collisionFlags = flags;
+ }
+
+ ///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm::
+ btScalar getCcdSweptSphereRadius() const
+ {
+ return m_ccdSweptSphereRadius;
+ }
+
+ ///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm::
+ void setCcdSweptSphereRadius(btScalar radius)
+ {
+ m_ccdSweptSphereRadius = radius;
+ }
+
+ btScalar getCcdMotionThreshold() const
+ {
+ return m_ccdMotionThreshold;
+ }
+
+ btScalar getCcdSquareMotionThreshold() const
+ {
+ return m_ccdMotionThreshold*m_ccdMotionThreshold;
+ }
+
+
+
+ /// Don't do continuous collision detection if the motion (in one step) is less then m_ccdMotionThreshold
+ void setCcdMotionThreshold(btScalar ccdMotionThreshold)
+ {
+ m_ccdMotionThreshold = ccdMotionThreshold;
+ }
+
+ ///users can point to their objects, userPointer is not used by Bullet
+ void* getUserPointer() const
+ {
+ return m_userObjectPointer;
+ }
+
+ ///users can point to their objects, userPointer is not used by Bullet
+ void setUserPointer(void* userPointer)
+ {
+ m_userObjectPointer = userPointer;
+ }
+
+
+ inline bool checkCollideWith(btCollisionObject* co)
+ {
+ if (m_checkCollideWith)
+ return checkCollideWithOverride(co);
+
+ return true;
+ }
+};
+
+#endif //COLLISION_OBJECT_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btCollisionWorld.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btCollisionWorld.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btCollisionWorld.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btCollisionWorld.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,422 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://bulletphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+/**
+ * @mainpage Bullet Documentation
+ *
+ * @section intro_sec Introduction
+ * Bullet Collision Detection & Physics SDK
+ *
+ * Bullet is a Collision Detection and Rigid Body Dynamics Library. The Library is Open Source and free for commercial use, under the ZLib license ( http://opensource.org/licenses/zlib-license.php ).
+ *
+ * There is the Physics Forum for feedback and general Collision Detection and Physics discussions.
+ * Please visit http://www.bulletphysics.com
+ *
+ * @section install_sec Installation
+ *
+ * @subsection step1 Step 1: Download
+ * You can download the Bullet Physics Library from the Google Code repository: http://code.google.com/p/bullet/downloads/list
+ * @subsection step2 Step 2: Building
+ * Bullet comes with autogenerated Project Files for Microsoft Visual Studio 6, 7, 7.1 and 8.
+ * The main Workspace/Solution is located in Bullet/msvc/8/wksbullet.sln (replace 8 with your version).
+ *
+ * Under other platforms, like Linux or Mac OS-X, Bullet can be build using either using make, cmake, http://www.cmake.org , or jam, http://www.perforce.com/jam/jam.html . cmake can autogenerate Xcode, KDevelop, MSVC and other build systems. just run cmake . in the root of Bullet.
+ * So if you are not using MSVC or cmake, you can run ./autogen.sh ./configure to create both Makefile and Jamfile and then run make or jam.
+ * Jam is a build system that can build the library, demos and also autogenerate the MSVC Project Files.
+ * If you don't have jam installed, you can make jam from the included jam-2.5 sources, or download jam from ftp://ftp.perforce.com/jam
+ *
+ * @subsection step3 Step 3: Testing demos
+ * Try to run and experiment with BasicDemo executable as a starting point.
+ * Bullet can be used in several ways, as Full Rigid Body simulation, as Collision Detector Library or Low Level / Snippets like the GJK Closest Point calculation.
+ * The Dependencies can be seen in this documentation under Directories
+ *
+ * @subsection step4 Step 4: Integrating in your application, full Rigid Body and Soft Body simulation
+ * Check out BasicDemo how to create a btDynamicsWorld, btRigidBody and btCollisionShape, Stepping the simulation and synchronizing your graphics object transform.
+ * Check out SoftDemo how to use soft body dynamics, using btSoftRigidDynamicsWorld.
+ * @subsection step5 Step 5 : Integrate the Collision Detection Library (without Dynamics and other Extras)
+ * Bullet Collision Detection can also be used without the Dynamics/Extras.
+ * Check out btCollisionWorld and btCollisionObject, and the CollisionInterfaceDemo.
+ * @subsection step6 Step 6 : Use Snippets like the GJK Closest Point calculation.
+ * Bullet has been designed in a modular way keeping dependencies to a minimum. The ConvexHullDistance demo demonstrates direct use of btGjkPairDetector.
+ *
+ * @section copyright Copyright
+ * Copyright (C) 2005-2008 Erwin Coumans, some contributions Copyright Gino van den Bergen, Christer Ericson, Simon Hobbs, Ricardo Padrela, F Richter(res), Stephane Redon
+ * Special thanks to all visitors of the Bullet Physics forum, and in particular above contributors, John McCutchan, Nathanael Presson, Dave Eberle, Dirk Gregorius, Erin Catto, Dave Eberle, Adam Moravanszky,
+ * Pierre Terdiman, Kenny Erleben, Russell Smith, Oliver Strunk, Jan Paul van Waveren, Marten Svanfeldt.
+ *
+ */
+
+
+
+#ifndef COLLISION_WORLD_H
+#define COLLISION_WORLD_H
+
+class btStackAlloc;
+class btCollisionShape;
+class btConvexShape;
+class btBroadphaseInterface;
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btTransform.h"
+#include "btCollisionObject.h"
+#include "btCollisionDispatcher.h"
+#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h"
+#include "LinearMath/btAlignedObjectArray.h"
+
+///CollisionWorld is interface and container for the collision detection
+class btCollisionWorld
+{
+
+
+protected:
+
+ btAlignedObjectArray<btCollisionObject*> m_collisionObjects;
+
+ btDispatcher* m_dispatcher1;
+
+ btDispatcherInfo m_dispatchInfo;
+
+ btStackAlloc* m_stackAlloc;
+
+ btBroadphaseInterface* m_broadphasePairCache;
+
+ btIDebugDraw* m_debugDrawer;
+
+ ///m_forceUpdateAllAabbs can be set to false as an optimization to only update active object AABBs
+ ///it is true by default, because it is error-prone (setting the position of static objects wouldn't update their AABB)
+ bool m_forceUpdateAllAabbs;
+
+public:
+
+ //this constructor doesn't own the dispatcher and paircache/broadphase
+ btCollisionWorld(btDispatcher* dispatcher,btBroadphaseInterface* broadphasePairCache, btCollisionConfiguration* collisionConfiguration);
+
+ virtual ~btCollisionWorld();
+
+ void setBroadphase(btBroadphaseInterface* pairCache)
+ {
+ m_broadphasePairCache = pairCache;
+ }
+
+ const btBroadphaseInterface* getBroadphase() const
+ {
+ return m_broadphasePairCache;
+ }
+
+ btBroadphaseInterface* getBroadphase()
+ {
+ return m_broadphasePairCache;
+ }
+
+ btOverlappingPairCache* getPairCache()
+ {
+ return m_broadphasePairCache->getOverlappingPairCache();
+ }
+
+
+ btDispatcher* getDispatcher()
+ {
+ return m_dispatcher1;
+ }
+
+ const btDispatcher* getDispatcher() const
+ {
+ return m_dispatcher1;
+ }
+
+ void updateSingleAabb(btCollisionObject* colObj);
+
+ virtual void updateAabbs();
+
+ virtual void setDebugDrawer(btIDebugDraw* debugDrawer)
+ {
+ m_debugDrawer = debugDrawer;
+ }
+
+ virtual btIDebugDraw* getDebugDrawer()
+ {
+ return m_debugDrawer;
+ }
+
+
+ ///LocalShapeInfo gives extra information for complex shapes
+ ///Currently, only btTriangleMeshShape is available, so it just contains triangleIndex and subpart
+ struct LocalShapeInfo
+ {
+ int m_shapePart;
+ int m_triangleIndex;
+
+ //const btCollisionShape* m_shapeTemp;
+ //const btTransform* m_shapeLocalTransform;
+ };
+
+ struct LocalRayResult
+ {
+ LocalRayResult(btCollisionObject* collisionObject,
+ LocalShapeInfo* localShapeInfo,
+ const btVector3& hitNormalLocal,
+ btScalar hitFraction)
+ :m_collisionObject(collisionObject),
+ m_localShapeInfo(localShapeInfo),
+ m_hitNormalLocal(hitNormalLocal),
+ m_hitFraction(hitFraction)
+ {
+ }
+
+ btCollisionObject* m_collisionObject;
+ LocalShapeInfo* m_localShapeInfo;
+ btVector3 m_hitNormalLocal;
+ btScalar m_hitFraction;
+
+ };
+
+ ///RayResultCallback is used to report new raycast results
+ struct RayResultCallback
+ {
+ btScalar m_closestHitFraction;
+ btCollisionObject* m_collisionObject;
+ short int m_collisionFilterGroup;
+ short int m_collisionFilterMask;
+ //@BP Mod - Custom flags, currently used to enable backface culling on tri-meshes, see btRaycastCallback
+ unsigned int m_flags;
+
+ virtual ~RayResultCallback()
+ {
+ }
+ bool hasHit() const
+ {
+ return (m_collisionObject != 0);
+ }
+
+ RayResultCallback()
+ :m_closestHitFraction(btScalar(1.)),
+ m_collisionObject(0),
+ m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter),
+ m_collisionFilterMask(btBroadphaseProxy::AllFilter),
+ //@BP Mod
+ m_flags(0)
+ {
+ }
+
+ virtual bool needsCollision(btBroadphaseProxy* proxy0) const
+ {
+ bool collides = (proxy0->m_collisionFilterGroup & m_collisionFilterMask) != 0;
+ collides = collides && (m_collisionFilterGroup & proxy0->m_collisionFilterMask);
+ return collides;
+ }
+
+
+ virtual btScalar addSingleResult(LocalRayResult& rayResult,bool normalInWorldSpace) = 0;
+ };
+
+ struct ClosestRayResultCallback : public RayResultCallback
+ {
+ ClosestRayResultCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld)
+ :m_rayFromWorld(rayFromWorld),
+ m_rayToWorld(rayToWorld)
+ {
+ }
+
+ btVector3 m_rayFromWorld;//used to calculate hitPointWorld from hitFraction
+ btVector3 m_rayToWorld;
+
+ btVector3 m_hitNormalWorld;
+ btVector3 m_hitPointWorld;
+
+ virtual btScalar addSingleResult(LocalRayResult& rayResult,bool normalInWorldSpace)
+ {
+ //caller already does the filter on the m_closestHitFraction
+ btAssert(rayResult.m_hitFraction <= m_closestHitFraction);
+
+ m_closestHitFraction = rayResult.m_hitFraction;
+ m_collisionObject = rayResult.m_collisionObject;
+ if (normalInWorldSpace)
+ {
+ m_hitNormalWorld = rayResult.m_hitNormalLocal;
+ } else
+ {
+ ///need to transform normal into worldspace
+ m_hitNormalWorld = m_collisionObject->getWorldTransform().getBasis()*rayResult.m_hitNormalLocal;
+ }
+ m_hitPointWorld.setInterpolate3(m_rayFromWorld,m_rayToWorld,rayResult.m_hitFraction);
+ return rayResult.m_hitFraction;
+ }
+ };
+
+
+ struct LocalConvexResult
+ {
+ LocalConvexResult(btCollisionObject* hitCollisionObject,
+ LocalShapeInfo* localShapeInfo,
+ const btVector3& hitNormalLocal,
+ const btVector3& hitPointLocal,
+ btScalar hitFraction
+ )
+ :m_hitCollisionObject(hitCollisionObject),
+ m_localShapeInfo(localShapeInfo),
+ m_hitNormalLocal(hitNormalLocal),
+ m_hitPointLocal(hitPointLocal),
+ m_hitFraction(hitFraction)
+ {
+ }
+
+ btCollisionObject* m_hitCollisionObject;
+ LocalShapeInfo* m_localShapeInfo;
+ btVector3 m_hitNormalLocal;
+ btVector3 m_hitPointLocal;
+ btScalar m_hitFraction;
+ };
+
+ ///RayResultCallback is used to report new raycast results
+ struct ConvexResultCallback
+ {
+ btScalar m_closestHitFraction;
+ short int m_collisionFilterGroup;
+ short int m_collisionFilterMask;
+
+ ConvexResultCallback()
+ :m_closestHitFraction(btScalar(1.)),
+ m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter),
+ m_collisionFilterMask(btBroadphaseProxy::AllFilter)
+ {
+ }
+
+ virtual ~ConvexResultCallback()
+ {
+ }
+
+ bool hasHit() const
+ {
+ return (m_closestHitFraction < btScalar(1.));
+ }
+
+
+
+ virtual bool needsCollision(btBroadphaseProxy* proxy0) const
+ {
+ bool collides = (proxy0->m_collisionFilterGroup & m_collisionFilterMask) != 0;
+ collides = collides && (m_collisionFilterGroup & proxy0->m_collisionFilterMask);
+ return collides;
+ }
+
+ virtual btScalar addSingleResult(LocalConvexResult& convexResult,bool normalInWorldSpace) = 0;
+ };
+
+ struct ClosestConvexResultCallback : public ConvexResultCallback
+ {
+ ClosestConvexResultCallback(const btVector3& convexFromWorld,const btVector3& convexToWorld)
+ :m_convexFromWorld(convexFromWorld),
+ m_convexToWorld(convexToWorld),
+ m_hitCollisionObject(0)
+ {
+ }
+
+ btVector3 m_convexFromWorld;//used to calculate hitPointWorld from hitFraction
+ btVector3 m_convexToWorld;
+
+ btVector3 m_hitNormalWorld;
+ btVector3 m_hitPointWorld;
+ btCollisionObject* m_hitCollisionObject;
+
+ virtual btScalar addSingleResult(LocalConvexResult& convexResult,bool normalInWorldSpace)
+ {
+//caller already does the filter on the m_closestHitFraction
+ btAssert(convexResult.m_hitFraction <= m_closestHitFraction);
+
+ m_closestHitFraction = convexResult.m_hitFraction;
+ m_hitCollisionObject = convexResult.m_hitCollisionObject;
+ if (normalInWorldSpace)
+ {
+ m_hitNormalWorld = convexResult.m_hitNormalLocal;
+ } else
+ {
+ ///need to transform normal into worldspace
+ m_hitNormalWorld = m_hitCollisionObject->getWorldTransform().getBasis()*convexResult.m_hitNormalLocal;
+ }
+ m_hitPointWorld = convexResult.m_hitPointLocal;
+ return convexResult.m_hitFraction;
+ }
+ };
+
+ int getNumCollisionObjects() const
+ {
+ return int(m_collisionObjects.size());
+ }
+
+ /// rayTest performs a raycast on all objects in the btCollisionWorld, and calls the resultCallback
+ /// This allows for several queries: first hit, all hits, any hit, dependent on the value returned by the callback.
+ void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const;
+
+ // convexTest performs a swept convex cast on all objects in the btCollisionWorld, and calls the resultCallback
+ // This allows for several queries: first hit, all hits, any hit, dependent on the value return by the callback.
+ void convexSweepTest (const btConvexShape* castShape, const btTransform& from, const btTransform& to, ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration = btScalar(0.)) const;
+
+
+ /// rayTestSingle performs a raycast call and calls the resultCallback. It is used internally by rayTest.
+ /// In a future implementation, we consider moving the ray test as a virtual method in btCollisionShape.
+ /// This allows more customization.
+ static void rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans,
+ btCollisionObject* collisionObject,
+ const btCollisionShape* collisionShape,
+ const btTransform& colObjWorldTransform,
+ RayResultCallback& resultCallback);
+
+ /// objectQuerySingle performs a collision detection query and calls the resultCallback. It is used internally by rayTest.
+ static void objectQuerySingle(const btConvexShape* castShape, const btTransform& rayFromTrans,const btTransform& rayToTrans,
+ btCollisionObject* collisionObject,
+ const btCollisionShape* collisionShape,
+ const btTransform& colObjWorldTransform,
+ ConvexResultCallback& resultCallback, btScalar allowedPenetration);
+
+ virtual void addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup=btBroadphaseProxy::DefaultFilter,short int collisionFilterMask=btBroadphaseProxy::AllFilter);
+
+ btCollisionObjectArray& getCollisionObjectArray()
+ {
+ return m_collisionObjects;
+ }
+
+ const btCollisionObjectArray& getCollisionObjectArray() const
+ {
+ return m_collisionObjects;
+ }
+
+
+ virtual void removeCollisionObject(btCollisionObject* collisionObject);
+
+ virtual void performDiscreteCollisionDetection();
+
+ btDispatcherInfo& getDispatchInfo()
+ {
+ return m_dispatchInfo;
+ }
+
+ const btDispatcherInfo& getDispatchInfo() const
+ {
+ return m_dispatchInfo;
+ }
+
+ bool getForceUpdateAllAabbs() const
+ {
+ return m_forceUpdateAllAabbs;
+ }
+ void setForceUpdateAllAabbs( bool forceUpdateAllAabbs)
+ {
+ m_forceUpdateAllAabbs = forceUpdateAllAabbs;
+ }
+
+};
+
+
+#endif //COLLISION_WORLD_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,86 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef COMPOUND_COLLISION_ALGORITHM_H
+#define COMPOUND_COLLISION_ALGORITHM_H
+
+#include "btActivatingCollisionAlgorithm.h"
+#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
+
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+class btDispatcher;
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "btCollisionCreateFunc.h"
+#include "LinearMath/btAlignedObjectArray.h"
+class btDispatcher;
+class btCollisionObject;
+
+/// btCompoundCollisionAlgorithm supports collision between CompoundCollisionShapes and other collision shapes
+class btCompoundCollisionAlgorithm : public btActivatingCollisionAlgorithm
+{
+ btAlignedObjectArray<btCollisionAlgorithm*> m_childCollisionAlgorithms;
+ bool m_isSwapped;
+
+ class btPersistentManifold* m_sharedManifold;
+ bool m_ownsManifold;
+
+ int m_compoundShapeRevision;//to keep track of changes, so that childAlgorithm array can be updated
+
+ void removeChildAlgorithms();
+
+ void preallocateChildAlgorithms(btCollisionObject* body0,btCollisionObject* body1);
+
+public:
+
+ btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped);
+
+ virtual ~btCompoundCollisionAlgorithm();
+
+ virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+ btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+ virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
+ {
+ int i;
+ for (i=0;i<m_childCollisionAlgorithms.size();i++)
+ {
+ if (m_childCollisionAlgorithms[i])
+ m_childCollisionAlgorithms[i]->getAllContactManifolds(manifoldArray);
+ }
+ }
+
+ struct CreateFunc :public btCollisionAlgorithmCreateFunc
+ {
+ virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+ {
+ void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btCompoundCollisionAlgorithm));
+ return new(mem) btCompoundCollisionAlgorithm(ci,body0,body1,false);
+ }
+ };
+
+ struct SwappedCreateFunc :public btCollisionAlgorithmCreateFunc
+ {
+ virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+ {
+ void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btCompoundCollisionAlgorithm));
+ return new(mem) btCompoundCollisionAlgorithm(ci,body0,body1,true);
+ }
+ };
+
+};
+
+#endif //COMPOUND_COLLISION_ALGORITHM_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,95 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef CONVEX_2D_CONVEX_2D_ALGORITHM_H
+#define CONVEX_2D_CONVEX_2D_ALGORITHM_H
+
+#include "BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
+#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "LinearMath/btTransformUtil.h" //for btConvexSeparatingDistanceUtil
+
+class btConvexPenetrationDepthSolver;
+
+
+///The convex2dConvex2dAlgorithm collision algorithm support 2d collision detection for btConvex2dShape
+///Currently it requires the btMinkowskiPenetrationDepthSolver, it has support for 2d penetration depth computation
+class btConvex2dConvex2dAlgorithm : public btActivatingCollisionAlgorithm
+{
+ btSimplexSolverInterface* m_simplexSolver;
+ btConvexPenetrationDepthSolver* m_pdSolver;
+
+
+ bool m_ownManifold;
+ btPersistentManifold* m_manifoldPtr;
+ bool m_lowLevelOfDetail;
+
+ int m_numPerturbationIterations;
+ int m_minimumPointsPerturbationThreshold;
+
+public:
+
+ btConvex2dConvex2dAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold);
+
+
+ virtual ~btConvex2dConvex2dAlgorithm();
+
+ virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+ virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+ virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
+ {
+ ///should we use m_ownManifold to avoid adding duplicates?
+ if (m_manifoldPtr && m_ownManifold)
+ manifoldArray.push_back(m_manifoldPtr);
+ }
+
+
+ void setLowLevelOfDetail(bool useLowLevel);
+
+
+ const btPersistentManifold* getManifold()
+ {
+ return m_manifoldPtr;
+ }
+
+ struct CreateFunc :public btCollisionAlgorithmCreateFunc
+ {
+
+ btConvexPenetrationDepthSolver* m_pdSolver;
+ btSimplexSolverInterface* m_simplexSolver;
+ int m_numPerturbationIterations;
+ int m_minimumPointsPerturbationThreshold;
+
+ CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver);
+
+ virtual ~CreateFunc();
+
+ virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+ {
+ void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvex2dConvex2dAlgorithm));
+ return new(mem) btConvex2dConvex2dAlgorithm(ci.m_manifold,ci,body0,body1,m_simplexSolver,m_pdSolver,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold);
+ }
+ };
+
+
+};
+
+#endif //CONVEX_2D_CONVEX_2D_ALGORITHM_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,116 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef CONVEX_CONCAVE_COLLISION_ALGORITHM_H
+#define CONVEX_CONCAVE_COLLISION_ALGORITHM_H
+
+#include "btActivatingCollisionAlgorithm.h"
+#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
+#include "BulletCollision/CollisionShapes/btTriangleCallback.h"
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+class btDispatcher;
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "btCollisionCreateFunc.h"
+
+///For each triangle in the concave mesh that overlaps with the AABB of a convex (m_convexProxy), processTriangle is called.
+class btConvexTriangleCallback : public btTriangleCallback
+{
+ btCollisionObject* m_convexBody;
+ btCollisionObject* m_triBody;
+
+ btVector3 m_aabbMin;
+ btVector3 m_aabbMax ;
+
+
+ btManifoldResult* m_resultOut;
+ btDispatcher* m_dispatcher;
+ const btDispatcherInfo* m_dispatchInfoPtr;
+ btScalar m_collisionMarginTriangle;
+
+public:
+int m_triangleCount;
+
+ btPersistentManifold* m_manifoldPtr;
+
+ btConvexTriangleCallback(btDispatcher* dispatcher,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped);
+
+ void setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+ virtual ~btConvexTriangleCallback();
+
+ virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex);
+
+ void clearCache();
+
+ SIMD_FORCE_INLINE const btVector3& getAabbMin() const
+ {
+ return m_aabbMin;
+ }
+ SIMD_FORCE_INLINE const btVector3& getAabbMax() const
+ {
+ return m_aabbMax;
+ }
+
+};
+
+
+
+
+/// btConvexConcaveCollisionAlgorithm supports collision between convex shapes and (concave) trianges meshes.
+class btConvexConcaveCollisionAlgorithm : public btActivatingCollisionAlgorithm
+{
+
+ bool m_isSwapped;
+
+ btConvexTriangleCallback m_btConvexTriangleCallback;
+
+
+
+public:
+
+ btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped);
+
+ virtual ~btConvexConcaveCollisionAlgorithm();
+
+ virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+ btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+ virtual void getAllContactManifolds(btManifoldArray& manifoldArray);
+
+ void clearCache();
+
+ struct CreateFunc :public btCollisionAlgorithmCreateFunc
+ {
+ virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+ {
+ void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexConcaveCollisionAlgorithm));
+ return new(mem) btConvexConcaveCollisionAlgorithm(ci,body0,body1,false);
+ }
+ };
+
+ struct SwappedCreateFunc :public btCollisionAlgorithmCreateFunc
+ {
+ virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+ {
+ void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexConcaveCollisionAlgorithm));
+ return new(mem) btConvexConcaveCollisionAlgorithm(ci,body0,body1,true);
+ }
+ };
+
+};
+
+#endif //CONVEX_CONCAVE_COLLISION_ALGORITHM_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,109 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef CONVEX_CONVEX_ALGORITHM_H
+#define CONVEX_CONVEX_ALGORITHM_H
+
+#include "btActivatingCollisionAlgorithm.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
+#include "btCollisionCreateFunc.h"
+#include "btCollisionDispatcher.h"
+#include "LinearMath/btTransformUtil.h" //for btConvexSeparatingDistanceUtil
+
+class btConvexPenetrationDepthSolver;
+
+///Enabling USE_SEPDISTANCE_UTIL2 requires 100% reliable distance computation. However, when using large size ratios GJK can be imprecise
+///so the distance is not conservative. In that case, enabling this USE_SEPDISTANCE_UTIL2 would result in failing/missing collisions.
+///Either improve GJK for large size ratios (testing a 100 units versus a 0.1 unit object) or only enable the util
+///for certain pairs that have a small size ratio
+
+#define USE_SEPDISTANCE_UTIL2 1
+
+///The convexConvexAlgorithm collision algorithm implements time of impact, convex closest points and penetration depth calculations between two convex objects.
+///Multiple contact points are calculated by perturbing the orientation of the smallest object orthogonal to the separating normal.
+///This idea was described by Gino van den Bergen in this forum topic http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=4&t=288&p=888#p888
+class btConvexConvexAlgorithm : public btActivatingCollisionAlgorithm
+{
+#ifdef USE_SEPDISTANCE_UTIL2
+ btConvexSeparatingDistanceUtil m_sepDistance;
+#endif
+ btSimplexSolverInterface* m_simplexSolver;
+ btConvexPenetrationDepthSolver* m_pdSolver;
+
+
+ bool m_ownManifold;
+ btPersistentManifold* m_manifoldPtr;
+ bool m_lowLevelOfDetail;
+
+ int m_numPerturbationIterations;
+ int m_minimumPointsPerturbationThreshold;
+
+
+ ///cache separating vector to speedup collision detection
+
+
+public:
+
+ btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold);
+
+
+ virtual ~btConvexConvexAlgorithm();
+
+ virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+ virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+ virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
+ {
+ ///should we use m_ownManifold to avoid adding duplicates?
+ if (m_manifoldPtr && m_ownManifold)
+ manifoldArray.push_back(m_manifoldPtr);
+ }
+
+
+ void setLowLevelOfDetail(bool useLowLevel);
+
+
+ const btPersistentManifold* getManifold()
+ {
+ return m_manifoldPtr;
+ }
+
+ struct CreateFunc :public btCollisionAlgorithmCreateFunc
+ {
+
+ btConvexPenetrationDepthSolver* m_pdSolver;
+ btSimplexSolverInterface* m_simplexSolver;
+ int m_numPerturbationIterations;
+ int m_minimumPointsPerturbationThreshold;
+
+ CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver);
+
+ virtual ~CreateFunc();
+
+ virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+ {
+ void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexConvexAlgorithm));
+ return new(mem) btConvexConvexAlgorithm(ci.m_manifold,ci,body0,body1,m_simplexSolver,m_pdSolver,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold);
+ }
+ };
+
+
+};
+
+#endif //CONVEX_CONVEX_ALGORITHM_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,84 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef CONVEX_PLANE_COLLISION_ALGORITHM_H
+#define CONVEX_PLANE_COLLISION_ALGORITHM_H
+
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
+class btPersistentManifold;
+#include "btCollisionDispatcher.h"
+
+#include "LinearMath/btVector3.h"
+
+/// btSphereBoxCollisionAlgorithm provides sphere-box collision detection.
+/// Other features are frame-coherency (persistent data) and collision response.
+class btConvexPlaneCollisionAlgorithm : public btCollisionAlgorithm
+{
+ bool m_ownManifold;
+ btPersistentManifold* m_manifoldPtr;
+ bool m_isSwapped;
+ int m_numPerturbationIterations;
+ int m_minimumPointsPerturbationThreshold;
+
+public:
+
+ btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped, int numPerturbationIterations,int minimumPointsPerturbationThreshold);
+
+ virtual ~btConvexPlaneCollisionAlgorithm();
+
+ virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+ void collideSingleContact (const btQuaternion& perturbeRot, btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+ virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+ virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
+ {
+ if (m_manifoldPtr && m_ownManifold)
+ {
+ manifoldArray.push_back(m_manifoldPtr);
+ }
+ }
+
+ struct CreateFunc :public btCollisionAlgorithmCreateFunc
+ {
+ int m_numPerturbationIterations;
+ int m_minimumPointsPerturbationThreshold;
+
+ CreateFunc()
+ : m_numPerturbationIterations(1),
+ m_minimumPointsPerturbationThreshold(1)
+ {
+ }
+
+ virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+ {
+ void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexPlaneCollisionAlgorithm));
+ if (!m_swapped)
+ {
+ return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,false,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold);
+ } else
+ {
+ return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,true,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold);
+ }
+ }
+ };
+
+};
+
+#endif //CONVEX_PLANE_COLLISION_ALGORITHM_H
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,130 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_DEFAULT_COLLISION_CONFIGURATION
+#define BT_DEFAULT_COLLISION_CONFIGURATION
+
+#include "btCollisionConfiguration.h"
+class btVoronoiSimplexSolver;
+class btConvexPenetrationDepthSolver;
+
+struct btDefaultCollisionConstructionInfo
+{
+ btStackAlloc* m_stackAlloc;
+ btPoolAllocator* m_persistentManifoldPool;
+ btPoolAllocator* m_collisionAlgorithmPool;
+ int m_defaultMaxPersistentManifoldPoolSize;
+ int m_defaultMaxCollisionAlgorithmPoolSize;
+ int m_customCollisionAlgorithmMaxElementSize;
+ int m_defaultStackAllocatorSize;
+ int m_useEpaPenetrationAlgorithm;
+
+ btDefaultCollisionConstructionInfo()
+ :m_stackAlloc(0),
+ m_persistentManifoldPool(0),
+ m_collisionAlgorithmPool(0),
+ m_defaultMaxPersistentManifoldPoolSize(4096),
+ m_defaultMaxCollisionAlgorithmPoolSize(4096),
+ m_customCollisionAlgorithmMaxElementSize(0),
+ m_defaultStackAllocatorSize(0),
+ m_useEpaPenetrationAlgorithm(true)
+ {
+ }
+};
+
+
+
+///btCollisionConfiguration allows to configure Bullet collision detection
+///stack allocator, pool memory allocators
+///@todo: describe the meaning
+class btDefaultCollisionConfiguration : public btCollisionConfiguration
+{
+
+protected:
+
+ int m_persistentManifoldPoolSize;
+
+ btStackAlloc* m_stackAlloc;
+ bool m_ownsStackAllocator;
+
+ btPoolAllocator* m_persistentManifoldPool;
+ bool m_ownsPersistentManifoldPool;
+
+
+ btPoolAllocator* m_collisionAlgorithmPool;
+ bool m_ownsCollisionAlgorithmPool;
+
+ //default simplex/penetration depth solvers
+ btVoronoiSimplexSolver* m_simplexSolver;
+ btConvexPenetrationDepthSolver* m_pdSolver;
+
+ //default CreationFunctions, filling the m_doubleDispatch table
+ btCollisionAlgorithmCreateFunc* m_convexConvexCreateFunc;
+ btCollisionAlgorithmCreateFunc* m_convexConcaveCreateFunc;
+ btCollisionAlgorithmCreateFunc* m_swappedConvexConcaveCreateFunc;
+ btCollisionAlgorithmCreateFunc* m_compoundCreateFunc;
+ btCollisionAlgorithmCreateFunc* m_swappedCompoundCreateFunc;
+ btCollisionAlgorithmCreateFunc* m_emptyCreateFunc;
+ btCollisionAlgorithmCreateFunc* m_sphereSphereCF;
+#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM
+ btCollisionAlgorithmCreateFunc* m_sphereBoxCF;
+ btCollisionAlgorithmCreateFunc* m_boxSphereCF;
+#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM
+
+ btCollisionAlgorithmCreateFunc* m_boxBoxCF;
+ btCollisionAlgorithmCreateFunc* m_sphereTriangleCF;
+ btCollisionAlgorithmCreateFunc* m_triangleSphereCF;
+ btCollisionAlgorithmCreateFunc* m_planeConvexCF;
+ btCollisionAlgorithmCreateFunc* m_convexPlaneCF;
+
+public:
+
+
+ btDefaultCollisionConfiguration(const btDefaultCollisionConstructionInfo& constructionInfo = btDefaultCollisionConstructionInfo());
+
+ virtual ~btDefaultCollisionConfiguration();
+
+ ///memory pools
+ virtual btPoolAllocator* getPersistentManifoldPool()
+ {
+ return m_persistentManifoldPool;
+ }
+
+ virtual btPoolAllocator* getCollisionAlgorithmPool()
+ {
+ return m_collisionAlgorithmPool;
+ }
+
+ virtual btStackAlloc* getStackAllocator()
+ {
+ return m_stackAlloc;
+ }
+
+
+ virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1);
+
+ ///Use this method to allow to generate multiple contact points between at once, between two objects using the generic convex-convex algorithm.
+ ///By default, this feature is disabled for best performance.
+ ///@param numPerturbationIterations controls the number of collision queries. Set it to zero to disable the feature.
+ ///@param minimumPointsPerturbationThreshold is the minimum number of points in the contact cache, above which the feature is disabled
+ ///3 is a good value for both params, if you want to enable the feature. This is because the default contact cache contains a maximum of 4 points, and one collision query at the unperturbed orientation is performed first.
+ ///See Bullet/Demos/CollisionDemo for an example how this feature gathers multiple points.
+ ///@todo we could add a per-object setting of those parameters, for level-of-detail collision detection.
+ void setConvexConvexMultipointIterations(int numPerturbationIterations=3, int minimumPointsPerturbationThreshold = 3);
+
+};
+
+#endif //BT_DEFAULT_COLLISION_CONFIGURATION
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,54 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef EMPTY_ALGORITH
+#define EMPTY_ALGORITH
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+#include "btCollisionCreateFunc.h"
+#include "btCollisionDispatcher.h"
+
+#define ATTRIBUTE_ALIGNED(a)
+
+///EmptyAlgorithm is a stub for unsupported collision pairs.
+///The dispatcher can dispatch a persistent btEmptyAlgorithm to avoid a search every frame.
+class btEmptyAlgorithm : public btCollisionAlgorithm
+{
+
+public:
+
+ btEmptyAlgorithm(const btCollisionAlgorithmConstructionInfo& ci);
+
+ virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+ virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+ virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
+ {
+ }
+
+ struct CreateFunc :public btCollisionAlgorithmCreateFunc
+ {
+ virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+ {
+ (void)body0;
+ (void)body1;
+ void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btEmptyAlgorithm));
+ return new(mem) btEmptyAlgorithm(ci);
+ }
+ };
+
+} ATTRIBUTE_ALIGNED(16);
+
+#endif //EMPTY_ALGORITH
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btGhostObject.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btGhostObject.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btGhostObject.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btGhostObject.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,174 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_GHOST_OBJECT_H
+#define BT_GHOST_OBJECT_H
+
+
+#include "btCollisionObject.h"
+#include "BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h"
+#include "LinearMath/btAlignedAllocator.h"
+#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h"
+#include "btCollisionWorld.h"
+
+class btConvexShape;
+
+class btDispatcher;
+
+///The btGhostObject can keep track of all objects that are overlapping
+///By default, this overlap is based on the AABB
+///This is useful for creating a character controller, collision sensors/triggers, explosions etc.
+///We plan on adding rayTest and other queries for the btGhostObject
+ATTRIBUTE_ALIGNED16(class) btGhostObject : public btCollisionObject
+{
+protected:
+
+ btAlignedObjectArray<btCollisionObject*> m_overlappingObjects;
+
+public:
+
+ btGhostObject();
+
+ virtual ~btGhostObject();
+
+ void convexSweepTest(const class btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, btCollisionWorld::ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration = 0.f) const;
+
+ void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, btCollisionWorld::RayResultCallback& resultCallback) const;
+
+ ///this method is mainly for expert/internal use only.
+ virtual void addOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btBroadphaseProxy* thisProxy=0);
+ ///this method is mainly for expert/internal use only.
+ virtual void removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy=0);
+
+ int getNumOverlappingObjects() const
+ {
+ return m_overlappingObjects.size();
+ }
+
+ btCollisionObject* getOverlappingObject(int index)
+ {
+ return m_overlappingObjects[index];
+ }
+
+ const btCollisionObject* getOverlappingObject(int index) const
+ {
+ return m_overlappingObjects[index];
+ }
+
+ btAlignedObjectArray<btCollisionObject*>& getOverlappingPairs()
+ {
+ return m_overlappingObjects;
+ }
+
+ const btAlignedObjectArray<btCollisionObject*> getOverlappingPairs() const
+ {
+ return m_overlappingObjects;
+ }
+
+ //
+ // internal cast
+ //
+
+ static const btGhostObject* upcast(const btCollisionObject* colObj)
+ {
+ if (colObj->getInternalType()==CO_GHOST_OBJECT)
+ return (const btGhostObject*)colObj;
+ return 0;
+ }
+ static btGhostObject* upcast(btCollisionObject* colObj)
+ {
+ if (colObj->getInternalType()==CO_GHOST_OBJECT)
+ return (btGhostObject*)colObj;
+ return 0;
+ }
+
+};
+
+class btPairCachingGhostObject : public btGhostObject
+{
+ btHashedOverlappingPairCache* m_hashPairCache;
+
+public:
+
+ btPairCachingGhostObject();
+
+ virtual ~btPairCachingGhostObject();
+
+ ///this method is mainly for expert/internal use only.
+ virtual void addOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btBroadphaseProxy* thisProxy=0);
+
+ virtual void removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy=0);
+
+ btHashedOverlappingPairCache* getOverlappingPairCache()
+ {
+ return m_hashPairCache;
+ }
+
+};
+
+
+
+///The btGhostPairCallback interfaces and forwards adding and removal of overlapping pairs from the btBroadphaseInterface to btGhostObject.
+class btGhostPairCallback : public btOverlappingPairCallback
+{
+
+public:
+ btGhostPairCallback()
+ {
+ }
+
+ virtual ~btGhostPairCallback()
+ {
+
+ }
+
+ virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+ {
+ btCollisionObject* colObj0 = (btCollisionObject*) proxy0->m_clientObject;
+ btCollisionObject* colObj1 = (btCollisionObject*) proxy1->m_clientObject;
+ btGhostObject* ghost0 = btGhostObject::upcast(colObj0);
+ btGhostObject* ghost1 = btGhostObject::upcast(colObj1);
+ if (ghost0)
+ ghost0->addOverlappingObjectInternal(proxy1, proxy0);
+ if (ghost1)
+ ghost1->addOverlappingObjectInternal(proxy0, proxy1);
+ return 0;
+ }
+
+ virtual void* removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher)
+ {
+ btCollisionObject* colObj0 = (btCollisionObject*) proxy0->m_clientObject;
+ btCollisionObject* colObj1 = (btCollisionObject*) proxy1->m_clientObject;
+ btGhostObject* ghost0 = btGhostObject::upcast(colObj0);
+ btGhostObject* ghost1 = btGhostObject::upcast(colObj1);
+ if (ghost0)
+ ghost0->removeOverlappingObjectInternal(proxy1,dispatcher,proxy0);
+ if (ghost1)
+ ghost1->removeOverlappingObjectInternal(proxy0,dispatcher,proxy1);
+ return 0;
+ }
+
+ virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy0,btDispatcher* dispatcher)
+ {
+ btAssert(0);
+ //need to keep track of all ghost objects and call them here
+ //m_hashPairCache->removeOverlappingPairsContainingProxy(proxy0,dispatcher);
+ }
+
+
+
+};
+
+#endif
\ No newline at end of file
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btManifoldResult.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btManifoldResult.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btManifoldResult.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btManifoldResult.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,126 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef MANIFOLD_RESULT_H
+#define MANIFOLD_RESULT_H
+
+class btCollisionObject;
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+class btManifoldPoint;
+
+#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
+
+#include "LinearMath/btTransform.h"
+
+typedef bool (*ContactAddedCallback)(btManifoldPoint& cp, const btCollisionObject* colObj0,int partId0,int index0,const btCollisionObject* colObj1,int partId1,int index1);
+extern ContactAddedCallback gContactAddedCallback;
+
+//#define DEBUG_PART_INDEX 1
+
+
+///btManifoldResult is a helper class to manage contact results.
+class btManifoldResult : public btDiscreteCollisionDetectorInterface::Result
+{
+ btPersistentManifold* m_manifoldPtr;
+
+ //we need this for compounds
+ btTransform m_rootTransA;
+ btTransform m_rootTransB;
+
+ btCollisionObject* m_body0;
+ btCollisionObject* m_body1;
+ int m_partId0;
+ int m_partId1;
+ int m_index0;
+ int m_index1;
+
+
+public:
+
+ btManifoldResult()
+#ifdef DEBUG_PART_INDEX
+ :
+ m_partId0(-1),
+ m_partId1(-1),
+ m_index0(-1),
+ m_index1(-1)
+#endif //DEBUG_PART_INDEX
+ {
+ }
+
+ btManifoldResult(btCollisionObject* body0,btCollisionObject* body1);
+
+ virtual ~btManifoldResult() {};
+
+ void setPersistentManifold(btPersistentManifold* manifoldPtr)
+ {
+ m_manifoldPtr = manifoldPtr;
+ }
+
+ const btPersistentManifold* getPersistentManifold() const
+ {
+ return m_manifoldPtr;
+ }
+ btPersistentManifold* getPersistentManifold()
+ {
+ return m_manifoldPtr;
+ }
+
+ virtual void setShapeIdentifiersA(int partId0,int index0)
+ {
+ m_partId0=partId0;
+ m_index0=index0;
+ }
+
+ virtual void setShapeIdentifiersB( int partId1,int index1)
+ {
+ m_partId1=partId1;
+ m_index1=index1;
+ }
+
+
+ virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth);
+
+ SIMD_FORCE_INLINE void refreshContactPoints()
+ {
+ btAssert(m_manifoldPtr);
+ if (!m_manifoldPtr->getNumContacts())
+ return;
+
+ bool isSwapped = m_manifoldPtr->getBody0() != m_body0;
+
+ if (isSwapped)
+ {
+ m_manifoldPtr->refreshContactPoints(m_rootTransB,m_rootTransA);
+ } else
+ {
+ m_manifoldPtr->refreshContactPoints(m_rootTransA,m_rootTransB);
+ }
+ }
+
+ const btCollisionObject* getBody0Internal() const
+ {
+ return m_body0;
+ }
+
+ const btCollisionObject* getBody1Internal() const
+ {
+ return m_body1;
+ }
+
+};
+
+#endif //MANIFOLD_RESULT_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btSimulationIslandManager.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btSimulationIslandManager.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btSimulationIslandManager.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btSimulationIslandManager.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,81 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef SIMULATION_ISLAND_MANAGER_H
+#define SIMULATION_ISLAND_MANAGER_H
+
+#include "BulletCollision/CollisionDispatch/btUnionFind.h"
+#include "btCollisionCreateFunc.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "btCollisionObject.h"
+
+class btCollisionObject;
+class btCollisionWorld;
+class btDispatcher;
+class btPersistentManifold;
+
+
+///SimulationIslandManager creates and handles simulation islands, using btUnionFind
+class btSimulationIslandManager
+{
+ btUnionFind m_unionFind;
+
+ btAlignedObjectArray<btPersistentManifold*> m_islandmanifold;
+ btAlignedObjectArray<btCollisionObject* > m_islandBodies;
+
+ bool m_splitIslands;
+
+public:
+ btSimulationIslandManager();
+ virtual ~btSimulationIslandManager();
+
+
+ void initUnionFind(int n);
+
+
+ btUnionFind& getUnionFind() { return m_unionFind;}
+
+ virtual void updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher);
+ virtual void storeIslandActivationState(btCollisionWorld* world);
+
+
+ void findUnions(btDispatcher* dispatcher,btCollisionWorld* colWorld);
+
+
+
+ struct IslandCallback
+ {
+ virtual ~IslandCallback() {};
+
+ virtual void ProcessIsland(btCollisionObject** bodies,int numBodies,class btPersistentManifold** manifolds,int numManifolds, int islandId) = 0;
+ };
+
+ void buildAndProcessIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld, IslandCallback* callback);
+
+ void buildIslands(btDispatcher* dispatcher,btCollisionWorld* colWorld);
+
+ bool getSplitIslands()
+ {
+ return m_splitIslands;
+ }
+ void setSplitIslands(bool doSplitIslands)
+ {
+ m_splitIslands = doSplitIslands;
+ }
+
+};
+
+#endif //SIMULATION_ISLAND_MANAGER_H
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,75 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef SPHERE_BOX_COLLISION_ALGORITHM_H
+#define SPHERE_BOX_COLLISION_ALGORITHM_H
+
+#include "btActivatingCollisionAlgorithm.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
+class btPersistentManifold;
+#include "btCollisionDispatcher.h"
+
+#include "LinearMath/btVector3.h"
+
+/// btSphereBoxCollisionAlgorithm provides sphere-box collision detection.
+/// Other features are frame-coherency (persistent data) and collision response.
+class btSphereBoxCollisionAlgorithm : public btActivatingCollisionAlgorithm
+{
+ bool m_ownManifold;
+ btPersistentManifold* m_manifoldPtr;
+ bool m_isSwapped;
+
+public:
+
+ btSphereBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped);
+
+ virtual ~btSphereBoxCollisionAlgorithm();
+
+ virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+ virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+ virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
+ {
+ if (m_manifoldPtr && m_ownManifold)
+ {
+ manifoldArray.push_back(m_manifoldPtr);
+ }
+ }
+
+ btScalar getSphereDistance( btCollisionObject* boxObj,btVector3& v3PointOnBox, btVector3& v3PointOnSphere, const btVector3& v3SphereCenter, btScalar fRadius );
+
+ btScalar getSpherePenetration( btCollisionObject* boxObj, btVector3& v3PointOnBox, btVector3& v3PointOnSphere, const btVector3& v3SphereCenter, btScalar fRadius, const btVector3& aabbMin, const btVector3& aabbMax);
+
+ struct CreateFunc :public btCollisionAlgorithmCreateFunc
+ {
+ virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+ {
+ void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereBoxCollisionAlgorithm));
+ if (!m_swapped)
+ {
+ return new(mem) btSphereBoxCollisionAlgorithm(0,ci,body0,body1,false);
+ } else
+ {
+ return new(mem) btSphereBoxCollisionAlgorithm(0,ci,body0,body1,true);
+ }
+ }
+ };
+
+};
+
+#endif //SPHERE_BOX_COLLISION_ALGORITHM_H
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,66 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef SPHERE_SPHERE_COLLISION_ALGORITHM_H
+#define SPHERE_SPHERE_COLLISION_ALGORITHM_H
+
+#include "btActivatingCollisionAlgorithm.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
+#include "btCollisionDispatcher.h"
+
+class btPersistentManifold;
+
+/// btSphereSphereCollisionAlgorithm provides sphere-sphere collision detection.
+/// Other features are frame-coherency (persistent data) and collision response.
+/// Also provides the most basic sample for custom/user btCollisionAlgorithm
+class btSphereSphereCollisionAlgorithm : public btActivatingCollisionAlgorithm
+{
+ bool m_ownManifold;
+ btPersistentManifold* m_manifoldPtr;
+
+public:
+ btSphereSphereCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1);
+
+ btSphereSphereCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
+ : btActivatingCollisionAlgorithm(ci) {}
+
+ virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+ virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+ virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
+ {
+ if (m_manifoldPtr && m_ownManifold)
+ {
+ manifoldArray.push_back(m_manifoldPtr);
+ }
+ }
+
+ virtual ~btSphereSphereCollisionAlgorithm();
+
+ struct CreateFunc :public btCollisionAlgorithmCreateFunc
+ {
+ virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+ {
+ void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereSphereCollisionAlgorithm));
+ return new(mem) btSphereSphereCollisionAlgorithm(0,ci,body0,body1);
+ }
+ };
+
+};
+
+#endif //SPHERE_SPHERE_COLLISION_ALGORITHM_H
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,69 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef SPHERE_TRIANGLE_COLLISION_ALGORITHM_H
+#define SPHERE_TRIANGLE_COLLISION_ALGORITHM_H
+
+#include "btActivatingCollisionAlgorithm.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
+class btPersistentManifold;
+#include "btCollisionDispatcher.h"
+
+/// btSphereSphereCollisionAlgorithm provides sphere-sphere collision detection.
+/// Other features are frame-coherency (persistent data) and collision response.
+/// Also provides the most basic sample for custom/user btCollisionAlgorithm
+class btSphereTriangleCollisionAlgorithm : public btActivatingCollisionAlgorithm
+{
+ bool m_ownManifold;
+ btPersistentManifold* m_manifoldPtr;
+ bool m_swapped;
+
+public:
+ btSphereTriangleCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool swapped);
+
+ btSphereTriangleCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
+ : btActivatingCollisionAlgorithm(ci) {}
+
+ virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+ virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+ virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
+ {
+ if (m_manifoldPtr && m_ownManifold)
+ {
+ manifoldArray.push_back(m_manifoldPtr);
+ }
+ }
+
+ virtual ~btSphereTriangleCollisionAlgorithm();
+
+ struct CreateFunc :public btCollisionAlgorithmCreateFunc
+ {
+
+ virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+ {
+
+ void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereTriangleCollisionAlgorithm));
+
+ return new(mem) btSphereTriangleCollisionAlgorithm(ci.m_manifold,ci,body0,body1,m_swapped);
+ }
+ };
+
+};
+
+#endif //SPHERE_TRIANGLE_COLLISION_ALGORITHM_H
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btUnionFind.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btUnionFind.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btUnionFind.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionDispatch/btUnionFind.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,124 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef UNION_FIND_H
+#define UNION_FIND_H
+
+#include "LinearMath/btAlignedObjectArray.h"
+
+ #define USE_PATH_COMPRESSION 1
+
+struct btElement
+{
+ int m_id;
+ int m_sz;
+};
+
+///UnionFind calculates connected subsets
+// Implements weighted Quick Union with path compression
+// optimization: could use short ints instead of ints (halving memory, would limit the number of rigid bodies to 64k, sounds reasonable)
+class btUnionFind
+ {
+ private:
+ btAlignedObjectArray<btElement> m_elements;
+
+ public:
+
+ btUnionFind();
+ ~btUnionFind();
+
+
+ //this is a special operation, destroying the content of btUnionFind.
+ //it sorts the elements, based on island id, in order to make it easy to iterate over islands
+ void sortIslands();
+
+ void reset(int N);
+
+ SIMD_FORCE_INLINE int getNumElements() const
+ {
+ return int(m_elements.size());
+ }
+ SIMD_FORCE_INLINE bool isRoot(int x) const
+ {
+ return (x == m_elements[x].m_id);
+ }
+
+ btElement& getElement(int index)
+ {
+ return m_elements[index];
+ }
+ const btElement& getElement(int index) const
+ {
+ return m_elements[index];
+ }
+
+ void allocate(int N);
+ void Free();
+
+
+
+
+ int find(int p, int q)
+ {
+ return (find(p) == find(q));
+ }
+
+ void unite(int p, int q)
+ {
+ int i = find(p), j = find(q);
+ if (i == j)
+ return;
+
+#ifndef USE_PATH_COMPRESSION
+ //weighted quick union, this keeps the 'trees' balanced, and keeps performance of unite O( log(n) )
+ if (m_elements[i].m_sz < m_elements[j].m_sz)
+ {
+ m_elements[i].m_id = j; m_elements[j].m_sz += m_elements[i].m_sz;
+ }
+ else
+ {
+ m_elements[j].m_id = i; m_elements[i].m_sz += m_elements[j].m_sz;
+ }
+#else
+ m_elements[i].m_id = j; m_elements[j].m_sz += m_elements[i].m_sz;
+#endif //USE_PATH_COMPRESSION
+ }
+
+ int find(int x)
+ {
+ //btAssert(x < m_N);
+ //btAssert(x >= 0);
+
+ while (x != m_elements[x].m_id)
+ {
+ //not really a reason not to use path compression, and it flattens the trees/improves find performance dramatically
+
+ #ifdef USE_PATH_COMPRESSION
+ //
+ m_elements[x].m_id = m_elements[m_elements[x].m_id].m_id;
+ #endif //
+ x = m_elements[x].m_id;
+ //btAssert(x < m_N);
+ //btAssert(x >= 0);
+
+ }
+ return x;
+ }
+
+
+ };
+
+
+#endif //UNION_FIND_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btBox2dShape.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btBox2dShape.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btBox2dShape.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btBox2dShape.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,363 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef OBB_BOX_2D_SHAPE_H
+#define OBB_BOX_2D_SHAPE_H
+
+#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
+#include "BulletCollision/CollisionShapes/btCollisionMargin.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btMinMax.h"
+
+///The btBox2dShape is a box primitive around the origin, its sides axis aligned with length specified by half extents, in local shape coordinates. When used as part of a btCollisionObject or btRigidBody it will be an oriented box in world space.
+class btBox2dShape: public btPolyhedralConvexShape
+{
+
+ //btVector3 m_boxHalfExtents1; //use m_implicitShapeDimensions instead
+
+ btVector3 m_centroid;
+ btVector3 m_vertices[4];
+ btVector3 m_normals[4];
+
+public:
+
+ btVector3 getHalfExtentsWithMargin() const
+ {
+ btVector3 halfExtents = getHalfExtentsWithoutMargin();
+ btVector3 margin(getMargin(),getMargin(),getMargin());
+ halfExtents += margin;
+ return halfExtents;
+ }
+
+ const btVector3& getHalfExtentsWithoutMargin() const
+ {
+ return m_implicitShapeDimensions;//changed in Bullet 2.63: assume the scaling and margin are included
+ }
+
+
+ virtual btVector3 localGetSupportingVertex(const btVector3& vec) const
+ {
+ btVector3 halfExtents = getHalfExtentsWithoutMargin();
+ btVector3 margin(getMargin(),getMargin(),getMargin());
+ halfExtents += margin;
+
+ return btVector3(btFsels(vec.x(), halfExtents.x(), -halfExtents.x()),
+ btFsels(vec.y(), halfExtents.y(), -halfExtents.y()),
+ btFsels(vec.z(), halfExtents.z(), -halfExtents.z()));
+ }
+
+ SIMD_FORCE_INLINE btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const
+ {
+ const btVector3& halfExtents = getHalfExtentsWithoutMargin();
+
+ return btVector3(btFsels(vec.x(), halfExtents.x(), -halfExtents.x()),
+ btFsels(vec.y(), halfExtents.y(), -halfExtents.y()),
+ btFsels(vec.z(), halfExtents.z(), -halfExtents.z()));
+ }
+
+ virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
+ {
+ const btVector3& halfExtents = getHalfExtentsWithoutMargin();
+
+ for (int i=0;i<numVectors;i++)
+ {
+ const btVector3& vec = vectors[i];
+ supportVerticesOut[i].setValue(btFsels(vec.x(), halfExtents.x(), -halfExtents.x()),
+ btFsels(vec.y(), halfExtents.y(), -halfExtents.y()),
+ btFsels(vec.z(), halfExtents.z(), -halfExtents.z()));
+ }
+
+ }
+
+
+ btBox2dShape( const btVector3& boxHalfExtents)
+ : btPolyhedralConvexShape(),
+ m_centroid(0,0,0)
+ {
+ m_vertices[0].setValue(-boxHalfExtents.getX(),-boxHalfExtents.getY(),0);
+ m_vertices[1].setValue(boxHalfExtents.getX(),-boxHalfExtents.getY(),0);
+ m_vertices[2].setValue(boxHalfExtents.getX(),boxHalfExtents.getY(),0);
+ m_vertices[3].setValue(-boxHalfExtents.getX(),boxHalfExtents.getY(),0);
+
+ m_normals[0].setValue(0,-1,0);
+ m_normals[1].setValue(1,0,0);
+ m_normals[2].setValue(0,1,0);
+ m_normals[3].setValue(-1,0,0);
+
+ m_shapeType = BOX_2D_SHAPE_PROXYTYPE;
+ btVector3 margin(getMargin(),getMargin(),getMargin());
+ m_implicitShapeDimensions = (boxHalfExtents * m_localScaling) - margin;
+ };
+
+ virtual void setMargin(btScalar collisionMargin)
+ {
+ //correct the m_implicitShapeDimensions for the margin
+ btVector3 oldMargin(getMargin(),getMargin(),getMargin());
+ btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin;
+
+ btConvexInternalShape::setMargin(collisionMargin);
+ btVector3 newMargin(getMargin(),getMargin(),getMargin());
+ m_implicitShapeDimensions = implicitShapeDimensionsWithMargin - newMargin;
+
+ }
+ virtual void setLocalScaling(const btVector3& scaling)
+ {
+ btVector3 oldMargin(getMargin(),getMargin(),getMargin());
+ btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin;
+ btVector3 unScaledImplicitShapeDimensionsWithMargin = implicitShapeDimensionsWithMargin / m_localScaling;
+
+ btConvexInternalShape::setLocalScaling(scaling);
+
+ m_implicitShapeDimensions = (unScaledImplicitShapeDimensionsWithMargin * m_localScaling) - oldMargin;
+
+ }
+
+ virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
+
+
+
+ virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+
+
+
+
+
+ int getVertexCount() const
+ {
+ return 4;
+ }
+
+ virtual int getNumVertices()const
+ {
+ return 4;
+ }
+
+ const btVector3* getVertices() const
+ {
+ return &m_vertices[0];
+ }
+
+ const btVector3* getNormals() const
+ {
+ return &m_normals[0];
+ }
+
+
+
+
+
+
+
+ virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const
+ {
+ //this plane might not be aligned...
+ btVector4 plane ;
+ getPlaneEquation(plane,i);
+ planeNormal = btVector3(plane.getX(),plane.getY(),plane.getZ());
+ planeSupport = localGetSupportingVertex(-planeNormal);
+ }
+
+
+ const btVector3& getCentroid() const
+ {
+ return m_centroid;
+ }
+
+ virtual int getNumPlanes() const
+ {
+ return 6;
+ }
+
+
+
+ virtual int getNumEdges() const
+ {
+ return 12;
+ }
+
+
+ virtual void getVertex(int i,btVector3& vtx) const
+ {
+ btVector3 halfExtents = getHalfExtentsWithoutMargin();
+
+ vtx = btVector3(
+ halfExtents.x() * (1-(i&1)) - halfExtents.x() * (i&1),
+ halfExtents.y() * (1-((i&2)>>1)) - halfExtents.y() * ((i&2)>>1),
+ halfExtents.z() * (1-((i&4)>>2)) - halfExtents.z() * ((i&4)>>2));
+ }
+
+
+ virtual void getPlaneEquation(btVector4& plane,int i) const
+ {
+ btVector3 halfExtents = getHalfExtentsWithoutMargin();
+
+ switch (i)
+ {
+ case 0:
+ plane.setValue(btScalar(1.),btScalar(0.),btScalar(0.),-halfExtents.x());
+ break;
+ case 1:
+ plane.setValue(btScalar(-1.),btScalar(0.),btScalar(0.),-halfExtents.x());
+ break;
+ case 2:
+ plane.setValue(btScalar(0.),btScalar(1.),btScalar(0.),-halfExtents.y());
+ break;
+ case 3:
+ plane.setValue(btScalar(0.),btScalar(-1.),btScalar(0.),-halfExtents.y());
+ break;
+ case 4:
+ plane.setValue(btScalar(0.),btScalar(0.),btScalar(1.),-halfExtents.z());
+ break;
+ case 5:
+ plane.setValue(btScalar(0.),btScalar(0.),btScalar(-1.),-halfExtents.z());
+ break;
+ default:
+ btAssert(0);
+ }
+ }
+
+
+ virtual void getEdge(int i,btVector3& pa,btVector3& pb) const
+ //virtual void getEdge(int i,Edge& edge) const
+ {
+ int edgeVert0 = 0;
+ int edgeVert1 = 0;
+
+ switch (i)
+ {
+ case 0:
+ edgeVert0 = 0;
+ edgeVert1 = 1;
+ break;
+ case 1:
+ edgeVert0 = 0;
+ edgeVert1 = 2;
+ break;
+ case 2:
+ edgeVert0 = 1;
+ edgeVert1 = 3;
+
+ break;
+ case 3:
+ edgeVert0 = 2;
+ edgeVert1 = 3;
+ break;
+ case 4:
+ edgeVert0 = 0;
+ edgeVert1 = 4;
+ break;
+ case 5:
+ edgeVert0 = 1;
+ edgeVert1 = 5;
+
+ break;
+ case 6:
+ edgeVert0 = 2;
+ edgeVert1 = 6;
+ break;
+ case 7:
+ edgeVert0 = 3;
+ edgeVert1 = 7;
+ break;
+ case 8:
+ edgeVert0 = 4;
+ edgeVert1 = 5;
+ break;
+ case 9:
+ edgeVert0 = 4;
+ edgeVert1 = 6;
+ break;
+ case 10:
+ edgeVert0 = 5;
+ edgeVert1 = 7;
+ break;
+ case 11:
+ edgeVert0 = 6;
+ edgeVert1 = 7;
+ break;
+ default:
+ btAssert(0);
+
+ }
+
+ getVertex(edgeVert0,pa );
+ getVertex(edgeVert1,pb );
+ }
+
+
+
+
+
+ virtual bool isInside(const btVector3& pt,btScalar tolerance) const
+ {
+ btVector3 halfExtents = getHalfExtentsWithoutMargin();
+
+ //btScalar minDist = 2*tolerance;
+
+ bool result = (pt.x() <= (halfExtents.x()+tolerance)) &&
+ (pt.x() >= (-halfExtents.x()-tolerance)) &&
+ (pt.y() <= (halfExtents.y()+tolerance)) &&
+ (pt.y() >= (-halfExtents.y()-tolerance)) &&
+ (pt.z() <= (halfExtents.z()+tolerance)) &&
+ (pt.z() >= (-halfExtents.z()-tolerance));
+
+ return result;
+ }
+
+
+ //debugging
+ virtual const char* getName()const
+ {
+ return "Box2d";
+ }
+
+ virtual int getNumPreferredPenetrationDirections() const
+ {
+ return 6;
+ }
+
+ virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const
+ {
+ switch (index)
+ {
+ case 0:
+ penetrationVector.setValue(btScalar(1.),btScalar(0.),btScalar(0.));
+ break;
+ case 1:
+ penetrationVector.setValue(btScalar(-1.),btScalar(0.),btScalar(0.));
+ break;
+ case 2:
+ penetrationVector.setValue(btScalar(0.),btScalar(1.),btScalar(0.));
+ break;
+ case 3:
+ penetrationVector.setValue(btScalar(0.),btScalar(-1.),btScalar(0.));
+ break;
+ case 4:
+ penetrationVector.setValue(btScalar(0.),btScalar(0.),btScalar(1.));
+ break;
+ case 5:
+ penetrationVector.setValue(btScalar(0.),btScalar(0.),btScalar(-1.));
+ break;
+ default:
+ btAssert(0);
+ }
+ }
+
+};
+
+#endif //OBB_BOX_2D_SHAPE_H
+
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btBoxShape.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btBoxShape.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btBoxShape.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btBoxShape.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,317 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef OBB_BOX_MINKOWSKI_H
+#define OBB_BOX_MINKOWSKI_H
+
+#include "btPolyhedralConvexShape.h"
+#include "btCollisionMargin.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btMinMax.h"
+
+///The btBoxShape is a box primitive around the origin, its sides axis aligned with length specified by half extents, in local shape coordinates. When used as part of a btCollisionObject or btRigidBody it will be an oriented box in world space.
+class btBoxShape: public btPolyhedralConvexShape
+{
+
+ //btVector3 m_boxHalfExtents1; //use m_implicitShapeDimensions instead
+
+
+public:
+
+ btVector3 getHalfExtentsWithMargin() const
+ {
+ btVector3 halfExtents = getHalfExtentsWithoutMargin();
+ btVector3 margin(getMargin(),getMargin(),getMargin());
+ halfExtents += margin;
+ return halfExtents;
+ }
+
+ const btVector3& getHalfExtentsWithoutMargin() const
+ {
+ return m_implicitShapeDimensions;//changed in Bullet 2.63: assume the scaling and margin are included
+ }
+
+
+ virtual btVector3 localGetSupportingVertex(const btVector3& vec) const
+ {
+ btVector3 halfExtents = getHalfExtentsWithoutMargin();
+ btVector3 margin(getMargin(),getMargin(),getMargin());
+ halfExtents += margin;
+
+ return btVector3(btFsels(vec.x(), halfExtents.x(), -halfExtents.x()),
+ btFsels(vec.y(), halfExtents.y(), -halfExtents.y()),
+ btFsels(vec.z(), halfExtents.z(), -halfExtents.z()));
+ }
+
+ SIMD_FORCE_INLINE btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const
+ {
+ const btVector3& halfExtents = getHalfExtentsWithoutMargin();
+
+ return btVector3(btFsels(vec.x(), halfExtents.x(), -halfExtents.x()),
+ btFsels(vec.y(), halfExtents.y(), -halfExtents.y()),
+ btFsels(vec.z(), halfExtents.z(), -halfExtents.z()));
+ }
+
+ virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
+ {
+ const btVector3& halfExtents = getHalfExtentsWithoutMargin();
+
+ for (int i=0;i<numVectors;i++)
+ {
+ const btVector3& vec = vectors[i];
+ supportVerticesOut[i].setValue(btFsels(vec.x(), halfExtents.x(), -halfExtents.x()),
+ btFsels(vec.y(), halfExtents.y(), -halfExtents.y()),
+ btFsels(vec.z(), halfExtents.z(), -halfExtents.z()));
+ }
+
+ }
+
+
+ btBoxShape( const btVector3& boxHalfExtents)
+ : btPolyhedralConvexShape()
+ {
+ m_shapeType = BOX_SHAPE_PROXYTYPE;
+ btVector3 margin(getMargin(),getMargin(),getMargin());
+ m_implicitShapeDimensions = (boxHalfExtents * m_localScaling) - margin;
+ };
+
+ virtual void setMargin(btScalar collisionMargin)
+ {
+ //correct the m_implicitShapeDimensions for the margin
+ btVector3 oldMargin(getMargin(),getMargin(),getMargin());
+ btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin;
+
+ btConvexInternalShape::setMargin(collisionMargin);
+ btVector3 newMargin(getMargin(),getMargin(),getMargin());
+ m_implicitShapeDimensions = implicitShapeDimensionsWithMargin - newMargin;
+
+ }
+ virtual void setLocalScaling(const btVector3& scaling)
+ {
+ btVector3 oldMargin(getMargin(),getMargin(),getMargin());
+ btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin;
+ btVector3 unScaledImplicitShapeDimensionsWithMargin = implicitShapeDimensionsWithMargin / m_localScaling;
+
+ btConvexInternalShape::setLocalScaling(scaling);
+
+ m_implicitShapeDimensions = (unScaledImplicitShapeDimensionsWithMargin * m_localScaling) - oldMargin;
+
+ }
+
+ virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
+
+
+
+ virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+
+ virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const
+ {
+ //this plane might not be aligned...
+ btVector4 plane ;
+ getPlaneEquation(plane,i);
+ planeNormal = btVector3(plane.getX(),plane.getY(),plane.getZ());
+ planeSupport = localGetSupportingVertex(-planeNormal);
+ }
+
+
+ virtual int getNumPlanes() const
+ {
+ return 6;
+ }
+
+ virtual int getNumVertices() const
+ {
+ return 8;
+ }
+
+ virtual int getNumEdges() const
+ {
+ return 12;
+ }
+
+
+ virtual void getVertex(int i,btVector3& vtx) const
+ {
+ btVector3 halfExtents = getHalfExtentsWithoutMargin();
+
+ vtx = btVector3(
+ halfExtents.x() * (1-(i&1)) - halfExtents.x() * (i&1),
+ halfExtents.y() * (1-((i&2)>>1)) - halfExtents.y() * ((i&2)>>1),
+ halfExtents.z() * (1-((i&4)>>2)) - halfExtents.z() * ((i&4)>>2));
+ }
+
+
+ virtual void getPlaneEquation(btVector4& plane,int i) const
+ {
+ btVector3 halfExtents = getHalfExtentsWithoutMargin();
+
+ switch (i)
+ {
+ case 0:
+ plane.setValue(btScalar(1.),btScalar(0.),btScalar(0.),-halfExtents.x());
+ break;
+ case 1:
+ plane.setValue(btScalar(-1.),btScalar(0.),btScalar(0.),-halfExtents.x());
+ break;
+ case 2:
+ plane.setValue(btScalar(0.),btScalar(1.),btScalar(0.),-halfExtents.y());
+ break;
+ case 3:
+ plane.setValue(btScalar(0.),btScalar(-1.),btScalar(0.),-halfExtents.y());
+ break;
+ case 4:
+ plane.setValue(btScalar(0.),btScalar(0.),btScalar(1.),-halfExtents.z());
+ break;
+ case 5:
+ plane.setValue(btScalar(0.),btScalar(0.),btScalar(-1.),-halfExtents.z());
+ break;
+ default:
+ btAssert(0);
+ }
+ }
+
+
+ virtual void getEdge(int i,btVector3& pa,btVector3& pb) const
+ //virtual void getEdge(int i,Edge& edge) const
+ {
+ int edgeVert0 = 0;
+ int edgeVert1 = 0;
+
+ switch (i)
+ {
+ case 0:
+ edgeVert0 = 0;
+ edgeVert1 = 1;
+ break;
+ case 1:
+ edgeVert0 = 0;
+ edgeVert1 = 2;
+ break;
+ case 2:
+ edgeVert0 = 1;
+ edgeVert1 = 3;
+
+ break;
+ case 3:
+ edgeVert0 = 2;
+ edgeVert1 = 3;
+ break;
+ case 4:
+ edgeVert0 = 0;
+ edgeVert1 = 4;
+ break;
+ case 5:
+ edgeVert0 = 1;
+ edgeVert1 = 5;
+
+ break;
+ case 6:
+ edgeVert0 = 2;
+ edgeVert1 = 6;
+ break;
+ case 7:
+ edgeVert0 = 3;
+ edgeVert1 = 7;
+ break;
+ case 8:
+ edgeVert0 = 4;
+ edgeVert1 = 5;
+ break;
+ case 9:
+ edgeVert0 = 4;
+ edgeVert1 = 6;
+ break;
+ case 10:
+ edgeVert0 = 5;
+ edgeVert1 = 7;
+ break;
+ case 11:
+ edgeVert0 = 6;
+ edgeVert1 = 7;
+ break;
+ default:
+ btAssert(0);
+
+ }
+
+ getVertex(edgeVert0,pa );
+ getVertex(edgeVert1,pb );
+ }
+
+
+
+
+
+ virtual bool isInside(const btVector3& pt,btScalar tolerance) const
+ {
+ btVector3 halfExtents = getHalfExtentsWithoutMargin();
+
+ //btScalar minDist = 2*tolerance;
+
+ bool result = (pt.x() <= (halfExtents.x()+tolerance)) &&
+ (pt.x() >= (-halfExtents.x()-tolerance)) &&
+ (pt.y() <= (halfExtents.y()+tolerance)) &&
+ (pt.y() >= (-halfExtents.y()-tolerance)) &&
+ (pt.z() <= (halfExtents.z()+tolerance)) &&
+ (pt.z() >= (-halfExtents.z()-tolerance));
+
+ return result;
+ }
+
+
+ //debugging
+ virtual const char* getName()const
+ {
+ return "Box";
+ }
+
+ virtual int getNumPreferredPenetrationDirections() const
+ {
+ return 6;
+ }
+
+ virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const
+ {
+ switch (index)
+ {
+ case 0:
+ penetrationVector.setValue(btScalar(1.),btScalar(0.),btScalar(0.));
+ break;
+ case 1:
+ penetrationVector.setValue(btScalar(-1.),btScalar(0.),btScalar(0.));
+ break;
+ case 2:
+ penetrationVector.setValue(btScalar(0.),btScalar(1.),btScalar(0.));
+ break;
+ case 3:
+ penetrationVector.setValue(btScalar(0.),btScalar(-1.),btScalar(0.));
+ break;
+ case 4:
+ penetrationVector.setValue(btScalar(0.),btScalar(0.),btScalar(1.));
+ break;
+ case 5:
+ penetrationVector.setValue(btScalar(0.),btScalar(0.),btScalar(-1.));
+ break;
+ default:
+ btAssert(0);
+ }
+ }
+
+};
+
+#endif //OBB_BOX_MINKOWSKI_H
+
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,86 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BVH_TRIANGLE_MESH_SHAPE_H
+#define BVH_TRIANGLE_MESH_SHAPE_H
+
+#include "btTriangleMeshShape.h"
+#include "btOptimizedBvh.h"
+#include "LinearMath/btAlignedAllocator.h"
+
+
+///The btBvhTriangleMeshShape is a static-triangle mesh shape with several optimizations, such as bounding volume hierarchy and cache friendly traversal for PlayStation 3 Cell SPU. It is recommended to enable useQuantizedAabbCompression for better memory usage.
+///It takes a triangle mesh as input, for example a btTriangleMesh or btTriangleIndexVertexArray. The btBvhTriangleMeshShape class allows for triangle mesh deformations by a refit or partialRefit method.
+///Instead of building the bounding volume hierarchy acceleration structure, it is also possible to serialize (save) and deserialize (load) the structure from disk.
+///See Demos\ConcaveDemo\ConcavePhysicsDemo.cpp for an example.
+ATTRIBUTE_ALIGNED16(class) btBvhTriangleMeshShape : public btTriangleMeshShape
+{
+
+ btOptimizedBvh* m_bvh;
+ bool m_useQuantizedAabbCompression;
+ bool m_ownsBvh;
+ bool m_pad[11];////need padding due to alignment
+
+public:
+
+ BT_DECLARE_ALIGNED_ALLOCATOR();
+
+ btBvhTriangleMeshShape() : btTriangleMeshShape(0),m_bvh(0),m_ownsBvh(false) {m_shapeType = TRIANGLE_MESH_SHAPE_PROXYTYPE;};
+ btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression, bool buildBvh = true);
+
+ ///optionally pass in a larger bvh aabb, used for quantization. This allows for deformations within this aabb
+ btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression,const btVector3& bvhAabbMin,const btVector3& bvhAabbMax, bool buildBvh = true);
+
+ virtual ~btBvhTriangleMeshShape();
+
+ bool getOwnsBvh () const
+ {
+ return m_ownsBvh;
+ }
+
+
+
+ void performRaycast (btTriangleCallback* callback, const btVector3& raySource, const btVector3& rayTarget);
+ void performConvexcast (btTriangleCallback* callback, const btVector3& boxSource, const btVector3& boxTarget, const btVector3& boxMin, const btVector3& boxMax);
+
+ virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const;
+
+ void refitTree(const btVector3& aabbMin,const btVector3& aabbMax);
+
+ ///for a fast incremental refit of parts of the tree. Note: the entire AABB of the tree will become more conservative, it never shrinks
+ void partialRefitTree(const btVector3& aabbMin,const btVector3& aabbMax);
+
+ //debugging
+ virtual const char* getName()const {return "BVHTRIANGLEMESH";}
+
+
+ virtual void setLocalScaling(const btVector3& scaling);
+
+ btOptimizedBvh* getOptimizedBvh()
+ {
+ return m_bvh;
+ }
+
+
+ void setOptimizedBvh(btOptimizedBvh* bvh, const btVector3& localScaling=btVector3(1,1,1));
+
+ bool usesQuantizedAabbCompression() const
+ {
+ return m_useQuantizedAabbCompression;
+ }
+}
+;
+
+#endif //BVH_TRIANGLE_MESH_SHAPE_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btCapsuleShape.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btCapsuleShape.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btCapsuleShape.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btCapsuleShape.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,129 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_CAPSULE_SHAPE_H
+#define BT_CAPSULE_SHAPE_H
+
+#include "btConvexInternalShape.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
+
+
+///The btCapsuleShape represents a capsule around the Y axis, there is also the btCapsuleShapeX aligned around the X axis and btCapsuleShapeZ around the Z axis.
+///The total height is height+2*radius, so the height is just the height between the center of each 'sphere' of the capsule caps.
+///The btCapsuleShape is a convex hull of two spheres. The btMultiSphereShape is a more general collision shape that takes the convex hull of multiple sphere, so it can also represent a capsule when just using two spheres.
+class btCapsuleShape : public btConvexInternalShape
+{
+protected:
+ int m_upAxis;
+
+protected:
+ ///only used for btCapsuleShapeZ and btCapsuleShapeX subclasses.
+ btCapsuleShape() : btConvexInternalShape() {m_shapeType = CAPSULE_SHAPE_PROXYTYPE;};
+
+public:
+ btCapsuleShape(btScalar radius,btScalar height);
+
+ ///CollisionShape Interface
+ virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+
+ /// btConvexShape Interface
+ virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
+
+ virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
+
+ virtual void getAabb (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
+ {
+ btVector3 halfExtents(getRadius(),getRadius(),getRadius());
+ halfExtents[m_upAxis] = getRadius() + getHalfHeight();
+ halfExtents += btVector3(getMargin(),getMargin(),getMargin());
+ btMatrix3x3 abs_b = t.getBasis().absolute();
+ btVector3 center = t.getOrigin();
+ btVector3 extent = btVector3(abs_b[0].dot(halfExtents),abs_b[1].dot(halfExtents),abs_b[2].dot(halfExtents));
+
+ aabbMin = center - extent;
+ aabbMax = center + extent;
+ }
+
+ virtual const char* getName()const
+ {
+ return "CapsuleShape";
+ }
+
+ int getUpAxis() const
+ {
+ return m_upAxis;
+ }
+
+ btScalar getRadius() const
+ {
+ int radiusAxis = (m_upAxis+2)%3;
+ return m_implicitShapeDimensions[radiusAxis];
+ }
+
+ btScalar getHalfHeight() const
+ {
+ return m_implicitShapeDimensions[m_upAxis];
+ }
+
+ virtual void setLocalScaling(const btVector3& scaling)
+ {
+ btVector3 oldMargin(getMargin(),getMargin(),getMargin());
+ btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin;
+ btVector3 unScaledImplicitShapeDimensionsWithMargin = implicitShapeDimensionsWithMargin / m_localScaling;
+
+ btConvexInternalShape::setLocalScaling(scaling);
+
+ m_implicitShapeDimensions = (unScaledImplicitShapeDimensionsWithMargin * m_localScaling) - oldMargin;
+
+ }
+};
+
+///btCapsuleShapeX represents a capsule around the Z axis
+///the total height is height+2*radius, so the height is just the height between the center of each 'sphere' of the capsule caps.
+class btCapsuleShapeX : public btCapsuleShape
+{
+public:
+
+ btCapsuleShapeX(btScalar radius,btScalar height);
+
+ //debugging
+ virtual const char* getName()const
+ {
+ return "CapsuleX";
+ }
+
+
+
+};
+
+///btCapsuleShapeZ represents a capsule around the Z axis
+///the total height is height+2*radius, so the height is just the height between the center of each 'sphere' of the capsule caps.
+class btCapsuleShapeZ : public btCapsuleShape
+{
+public:
+ btCapsuleShapeZ(btScalar radius,btScalar height);
+
+ //debugging
+ virtual const char* getName()const
+ {
+ return "CapsuleZ";
+ }
+
+
+};
+
+
+
+#endif //BT_CAPSULE_SHAPE_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btCollisionMargin.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btCollisionMargin.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btCollisionMargin.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btCollisionMargin.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,26 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef COLLISION_MARGIN_H
+#define COLLISION_MARGIN_H
+
+//used by Gjk and some other algorithms
+
+#define CONVEX_DISTANCE_MARGIN btScalar(0.04)// btScalar(0.1)//;//btScalar(0.01)
+
+
+
+#endif //COLLISION_MARGIN_H
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btCollisionShape.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btCollisionShape.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btCollisionShape.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btCollisionShape.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,117 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef COLLISION_SHAPE_H
+#define COLLISION_SHAPE_H
+
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btMatrix3x3.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" //for the shape types
+
+///The btCollisionShape class provides an interface for collision shapes that can be shared among btCollisionObjects.
+class btCollisionShape
+{
+protected:
+ int m_shapeType;
+ void* m_userPointer;
+
+public:
+
+ btCollisionShape() : m_shapeType (INVALID_SHAPE_PROXYTYPE), m_userPointer(0)
+ {
+ }
+
+ virtual ~btCollisionShape()
+ {
+ }
+
+ ///getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t.
+ virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const =0;
+
+ virtual void getBoundingSphere(btVector3& center,btScalar& radius) const;
+
+ ///getAngularMotionDisc returns the maximus radius needed for Conservative Advancement to handle time-of-impact with rotations.
+ virtual btScalar getAngularMotionDisc() const;
+
+ virtual btScalar getContactBreakingThreshold() const;
+
+
+ ///calculateTemporalAabb calculates the enclosing aabb for the moving object over interval [0..timeStep)
+ ///result is conservative
+ void calculateTemporalAabb(const btTransform& curTrans,const btVector3& linvel,const btVector3& angvel,btScalar timeStep, btVector3& temporalAabbMin,btVector3& temporalAabbMax) const;
+
+#ifndef __SPU__
+
+ SIMD_FORCE_INLINE bool isPolyhedral() const
+ {
+ return btBroadphaseProxy::isPolyhedral(getShapeType());
+ }
+
+ SIMD_FORCE_INLINE bool isConvex2d() const
+ {
+ return btBroadphaseProxy::isConvex2d(getShapeType());
+ }
+
+ SIMD_FORCE_INLINE bool isConvex() const
+ {
+ return btBroadphaseProxy::isConvex(getShapeType());
+ }
+ SIMD_FORCE_INLINE bool isConcave() const
+ {
+ return btBroadphaseProxy::isConcave(getShapeType());
+ }
+ SIMD_FORCE_INLINE bool isCompound() const
+ {
+ return btBroadphaseProxy::isCompound(getShapeType());
+ }
+
+ ///isInfinite is used to catch simulation error (aabb check)
+ SIMD_FORCE_INLINE bool isInfinite() const
+ {
+ return btBroadphaseProxy::isInfinite(getShapeType());
+ }
+
+
+ virtual void setLocalScaling(const btVector3& scaling) =0;
+ virtual const btVector3& getLocalScaling() const =0;
+ virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const = 0;
+
+
+//debugging support
+ virtual const char* getName()const =0 ;
+#endif //__SPU__
+
+
+ int getShapeType() const { return m_shapeType; }
+ virtual void setMargin(btScalar margin) = 0;
+ virtual btScalar getMargin() const = 0;
+
+
+ ///optional user data pointer
+ void setUserPointer(void* userPtr)
+ {
+ m_userPointer = userPtr;
+ }
+
+ void* getUserPointer() const
+ {
+ return m_userPointer;
+ }
+
+};
+
+#endif //COLLISION_SHAPE_H
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btCompoundShape.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btCompoundShape.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btCompoundShape.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btCompoundShape.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,172 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef COMPOUND_SHAPE_H
+#define COMPOUND_SHAPE_H
+
+#include "btCollisionShape.h"
+
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btMatrix3x3.h"
+#include "btCollisionMargin.h"
+#include "LinearMath/btAlignedObjectArray.h"
+
+//class btOptimizedBvh;
+struct btDbvt;
+
+ATTRIBUTE_ALIGNED16(struct) btCompoundShapeChild
+{
+ BT_DECLARE_ALIGNED_ALLOCATOR();
+
+ btTransform m_transform;
+ btCollisionShape* m_childShape;
+ int m_childShapeType;
+ btScalar m_childMargin;
+ struct btDbvtNode* m_node;
+};
+
+SIMD_FORCE_INLINE bool operator==(const btCompoundShapeChild& c1, const btCompoundShapeChild& c2)
+{
+ return ( c1.m_transform == c2.m_transform &&
+ c1.m_childShape == c2.m_childShape &&
+ c1.m_childShapeType == c2.m_childShapeType &&
+ c1.m_childMargin == c2.m_childMargin );
+}
+
+/// The btCompoundShape allows to store multiple other btCollisionShapes
+/// This allows for moving concave collision objects. This is more general then the static concave btBvhTriangleMeshShape.
+/// It has an (optional) dynamic aabb tree to accelerate early rejection tests.
+/// @todo: This aabb tree can also be use to speed up ray tests on btCompoundShape, see http://code.google.com/p/bullet/issues/detail?id=25
+/// Currently, removal of child shapes is only supported when disabling the aabb tree (pass 'false' in the constructor of btCompoundShape)
+ATTRIBUTE_ALIGNED16(class) btCompoundShape : public btCollisionShape
+{
+ btAlignedObjectArray<btCompoundShapeChild> m_children;
+ btVector3 m_localAabbMin;
+ btVector3 m_localAabbMax;
+
+ btDbvt* m_dynamicAabbTree;
+
+ ///increment m_updateRevision when adding/removing/replacing child shapes, so that some caches can be updated
+ int m_updateRevision;
+
+public:
+ BT_DECLARE_ALIGNED_ALLOCATOR();
+
+ btCompoundShape(bool enableDynamicAabbTree = true);
+
+ virtual ~btCompoundShape();
+
+ void addChildShape(const btTransform& localTransform,btCollisionShape* shape);
+
+ /// Remove all children shapes that contain the specified shape
+ virtual void removeChildShape(btCollisionShape* shape);
+
+ void removeChildShapeByIndex(int childShapeindex);
+
+
+ int getNumChildShapes() const
+ {
+ return int (m_children.size());
+ }
+
+ btCollisionShape* getChildShape(int index)
+ {
+ return m_children[index].m_childShape;
+ }
+ const btCollisionShape* getChildShape(int index) const
+ {
+ return m_children[index].m_childShape;
+ }
+
+ btTransform& getChildTransform(int index)
+ {
+ return m_children[index].m_transform;
+ }
+ const btTransform& getChildTransform(int index) const
+ {
+ return m_children[index].m_transform;
+ }
+
+ ///set a new transform for a child, and update internal data structures (local aabb and dynamic tree)
+ void updateChildTransform(int childIndex, const btTransform& newChildTransform);
+
+
+ btCompoundShapeChild* getChildList()
+ {
+ return &m_children[0];
+ }
+
+ ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
+ virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
+
+ /** Re-calculate the local Aabb. Is called at the end of removeChildShapes.
+ Use this yourself if you modify the children or their transforms. */
+ virtual void recalculateLocalAabb();
+
+ virtual void setLocalScaling(const btVector3& scaling)
+ {
+ m_localScaling = scaling;
+ }
+ virtual const btVector3& getLocalScaling() const
+ {
+ return m_localScaling;
+ }
+
+ virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+
+ virtual void setMargin(btScalar margin)
+ {
+ m_collisionMargin = margin;
+ }
+ virtual btScalar getMargin() const
+ {
+ return m_collisionMargin;
+ }
+ virtual const char* getName()const
+ {
+ return "Compound";
+ }
+
+ //this is optional, but should make collision queries faster, by culling non-overlapping nodes
+ void createAabbTreeFromChildren();
+
+ btDbvt* getDynamicAabbTree()
+ {
+ return m_dynamicAabbTree;
+ }
+
+ ///computes the exact moment of inertia and the transform from the coordinate system defined by the principal axes of the moment of inertia
+ ///and the center of mass to the current coordinate system. "masses" points to an array of masses of the children. The resulting transform
+ ///"principal" has to be applied inversely to all children transforms in order for the local coordinate system of the compound
+ ///shape to be centered at the center of mass and to coincide with the principal axes. This also necessitates a correction of the world transform
+ ///of the collision object by the principal transform.
+ void calculatePrincipalAxisTransform(btScalar* masses, btTransform& principal, btVector3& inertia) const;
+
+ int getUpdateRevision() const
+ {
+ return m_updateRevision;
+ }
+
+private:
+ btScalar m_collisionMargin;
+protected:
+ btVector3 m_localScaling;
+
+};
+
+
+
+#endif //COMPOUND_SHAPE_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btConcaveShape.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btConcaveShape.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btConcaveShape.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btConcaveShape.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,60 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef CONCAVE_SHAPE_H
+#define CONCAVE_SHAPE_H
+
+#include "btCollisionShape.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
+#include "btTriangleCallback.h"
+
+/// PHY_ScalarType enumerates possible scalar types.
+/// See the btStridingMeshInterface or btHeightfieldTerrainShape for its use
+typedef enum PHY_ScalarType {
+ PHY_FLOAT,
+ PHY_DOUBLE,
+ PHY_INTEGER,
+ PHY_SHORT,
+ PHY_FIXEDPOINT88,
+ PHY_UCHAR
+} PHY_ScalarType;
+
+///The btConcaveShape class provides an interface for non-moving (static) concave shapes.
+///It has been implemented by the btStaticPlaneShape, btBvhTriangleMeshShape and btHeightfieldTerrainShape.
+class btConcaveShape : public btCollisionShape
+{
+protected:
+ btScalar m_collisionMargin;
+
+public:
+ btConcaveShape();
+
+ virtual ~btConcaveShape();
+
+ virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const = 0;
+
+ virtual btScalar getMargin() const {
+ return m_collisionMargin;
+ }
+ virtual void setMargin(btScalar collisionMargin)
+ {
+ m_collisionMargin = collisionMargin;
+ }
+
+
+
+};
+
+#endif //CONCAVE_SHAPE_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btConeShape.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btConeShape.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btConeShape.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btConeShape.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,100 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef CONE_MINKOWSKI_H
+#define CONE_MINKOWSKI_H
+
+#include "btConvexInternalShape.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
+
+///The btConeShape implements a cone shape primitive, centered around the origin and aligned with the Y axis. The btConeShapeX is aligned around the X axis and btConeShapeZ around the Z axis.
+class btConeShape : public btConvexInternalShape
+
+{
+
+ btScalar m_sinAngle;
+ btScalar m_radius;
+ btScalar m_height;
+ int m_coneIndices[3];
+ btVector3 coneLocalSupport(const btVector3& v) const;
+
+
+public:
+ btConeShape (btScalar radius,btScalar height);
+
+ virtual btVector3 localGetSupportingVertex(const btVector3& vec) const;
+ virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const;
+ virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
+
+ btScalar getRadius() const { return m_radius;}
+ btScalar getHeight() const { return m_height;}
+
+
+ virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const
+ {
+ btTransform identity;
+ identity.setIdentity();
+ btVector3 aabbMin,aabbMax;
+ getAabb(identity,aabbMin,aabbMax);
+
+ btVector3 halfExtents = (aabbMax-aabbMin)*btScalar(0.5);
+
+ btScalar margin = getMargin();
+
+ btScalar lx=btScalar(2.)*(halfExtents.x()+margin);
+ btScalar ly=btScalar(2.)*(halfExtents.y()+margin);
+ btScalar lz=btScalar(2.)*(halfExtents.z()+margin);
+ const btScalar x2 = lx*lx;
+ const btScalar y2 = ly*ly;
+ const btScalar z2 = lz*lz;
+ const btScalar scaledmass = mass * btScalar(0.08333333);
+
+ inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2));
+
+// inertia.x() = scaledmass * (y2+z2);
+// inertia.y() = scaledmass * (x2+z2);
+// inertia.z() = scaledmass * (x2+y2);
+ }
+
+
+ virtual const char* getName()const
+ {
+ return "Cone";
+ }
+
+ ///choose upAxis index
+ void setConeUpIndex(int upIndex);
+
+ int getConeUpIndex() const
+ {
+ return m_coneIndices[1];
+ }
+};
+
+///btConeShape implements a Cone shape, around the X axis
+class btConeShapeX : public btConeShape
+{
+ public:
+ btConeShapeX(btScalar radius,btScalar height);
+};
+
+///btConeShapeZ implements a Cone shape, around the Z axis
+class btConeShapeZ : public btConeShape
+{
+ public:
+ btConeShapeZ(btScalar radius,btScalar height);
+};
+#endif //CONE_MINKOWSKI_H
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btConvex2dShape.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btConvex2dShape.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btConvex2dShape.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btConvex2dShape.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,80 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_CONVEX_2D_SHAPE_H
+#define BT_CONVEX_2D_SHAPE_H
+
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
+
+///The btConvex2dShape allows to use arbitrary convex shapes are 2d convex shapes, with the Z component assumed to be 0.
+///For 2d boxes, the btBox2dShape is recommended.
+class btConvex2dShape : public btConvexShape
+{
+ btConvexShape* m_childConvexShape;
+
+ public:
+
+ btConvex2dShape( btConvexShape* convexChildShape);
+
+ virtual ~btConvex2dShape();
+
+ virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
+
+ virtual btVector3 localGetSupportingVertex(const btVector3& vec)const;
+
+ virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
+
+ virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+
+ btConvexShape* getChildShape()
+ {
+ return m_childConvexShape;
+ }
+
+ const btConvexShape* getChildShape() const
+ {
+ return m_childConvexShape;
+ }
+
+ virtual const char* getName()const
+ {
+ return "Convex2dShape";
+ }
+
+
+
+ ///////////////////////////
+
+
+ ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
+ void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
+
+ virtual void getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
+
+ virtual void setLocalScaling(const btVector3& scaling) ;
+ virtual const btVector3& getLocalScaling() const ;
+
+ virtual void setMargin(btScalar margin);
+ virtual btScalar getMargin() const;
+
+ virtual int getNumPreferredPenetrationDirections() const;
+
+ virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const;
+
+
+};
+
+#endif //BT_CONVEX_2D_SHAPE_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btConvexHullShape.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btConvexHullShape.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btConvexHullShape.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btConvexHullShape.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,96 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef CONVEX_HULL_SHAPE_H
+#define CONVEX_HULL_SHAPE_H
+
+#include "btPolyhedralConvexShape.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
+#include "LinearMath/btAlignedObjectArray.h"
+
+
+///The btConvexHullShape implements an implicit convex hull of an array of vertices.
+///Bullet provides a general and fast collision detector for convex shapes based on GJK and EPA using localGetSupportingVertex.
+ATTRIBUTE_ALIGNED16(class) btConvexHullShape : public btPolyhedralConvexAabbCachingShape
+{
+ btAlignedObjectArray<btVector3> m_unscaledPoints;
+
+public:
+ BT_DECLARE_ALIGNED_ALLOCATOR();
+
+
+ ///this constructor optionally takes in a pointer to points. Each point is assumed to be 3 consecutive btScalar (x,y,z), the striding defines the number of bytes between each point, in memory.
+ ///It is easier to not pass any points in the constructor, and just add one point at a time, using addPoint.
+ ///btConvexHullShape make an internal copy of the points.
+ btConvexHullShape(const btScalar* points=0,int numPoints=0, int stride=sizeof(btVector3));
+
+ void addPoint(const btVector3& point);
+
+
+ btVector3* getUnscaledPoints()
+ {
+ return &m_unscaledPoints[0];
+ }
+
+ const btVector3* getUnscaledPoints() const
+ {
+ return &m_unscaledPoints[0];
+ }
+
+ ///getPoints is obsolete, please use getUnscaledPoints
+ const btVector3* getPoints() const
+ {
+ return getUnscaledPoints();
+ }
+
+
+
+
+ SIMD_FORCE_INLINE btVector3 getScaledPoint(int i) const
+ {
+ return m_unscaledPoints[i] * m_localScaling;
+ }
+
+ SIMD_FORCE_INLINE int getNumPoints() const
+ {
+ return m_unscaledPoints.size();
+ }
+
+ virtual btVector3 localGetSupportingVertex(const btVector3& vec)const;
+ virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
+ virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
+
+
+
+ //debugging
+ virtual const char* getName()const {return "Convex";}
+
+
+ virtual int getNumVertices() const;
+ virtual int getNumEdges() const;
+ virtual void getEdge(int i,btVector3& pa,btVector3& pb) const;
+ virtual void getVertex(int i,btVector3& vtx) const;
+ virtual int getNumPlanes() const;
+ virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const;
+ virtual bool isInside(const btVector3& pt,btScalar tolerance) const;
+
+ ///in case we receive negative scaling
+ virtual void setLocalScaling(const btVector3& scaling);
+
+};
+
+
+#endif //CONVEX_HULL_SHAPE_H
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btConvexInternalShape.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btConvexInternalShape.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btConvexInternalShape.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btConvexInternalShape.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,151 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_CONVEX_INTERNAL_SHAPE_H
+#define BT_CONVEX_INTERNAL_SHAPE_H
+
+#include "btConvexShape.h"
+#include "LinearMath/btAabbUtil2.h"
+
+///The btConvexInternalShape is an internal base class, shared by most convex shape implementations.
+class btConvexInternalShape : public btConvexShape
+{
+
+ protected:
+
+ //local scaling. collisionMargin is not scaled !
+ btVector3 m_localScaling;
+
+ btVector3 m_implicitShapeDimensions;
+
+ btScalar m_collisionMargin;
+
+ btScalar m_padding;
+
+ btConvexInternalShape();
+
+public:
+
+
+
+ virtual ~btConvexInternalShape()
+ {
+
+ }
+
+ virtual btVector3 localGetSupportingVertex(const btVector3& vec)const;
+
+ const btVector3& getImplicitShapeDimensions() const
+ {
+ return m_implicitShapeDimensions;
+ }
+
+ ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
+ void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
+ {
+ getAabbSlow(t,aabbMin,aabbMax);
+ }
+
+
+
+ virtual void getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
+
+
+ virtual void setLocalScaling(const btVector3& scaling);
+ virtual const btVector3& getLocalScaling() const
+ {
+ return m_localScaling;
+ }
+
+ const btVector3& getLocalScalingNV() const
+ {
+ return m_localScaling;
+ }
+
+ virtual void setMargin(btScalar margin)
+ {
+ m_collisionMargin = margin;
+ }
+ virtual btScalar getMargin() const
+ {
+ return m_collisionMargin;
+ }
+
+ btScalar getMarginNV() const
+ {
+ return m_collisionMargin;
+ }
+
+ virtual int getNumPreferredPenetrationDirections() const
+ {
+ return 0;
+ }
+
+ virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const
+ {
+ (void)penetrationVector;
+ (void)index;
+ btAssert(0);
+ }
+
+
+
+};
+
+
+///btConvexInternalAabbCachingShape adds local aabb caching for convex shapes, to avoid expensive bounding box calculations
+class btConvexInternalAabbCachingShape : public btConvexInternalShape
+{
+ btVector3 m_localAabbMin;
+ btVector3 m_localAabbMax;
+ bool m_isLocalAabbValid;
+
+protected:
+
+ btConvexInternalAabbCachingShape();
+
+ void setCachedLocalAabb (const btVector3& aabbMin, const btVector3& aabbMax)
+ {
+ m_isLocalAabbValid = true;
+ m_localAabbMin = aabbMin;
+ m_localAabbMax = aabbMax;
+ }
+
+ inline void getCachedLocalAabb (btVector3& aabbMin, btVector3& aabbMax) const
+ {
+ btAssert(m_isLocalAabbValid);
+ aabbMin = m_localAabbMin;
+ aabbMax = m_localAabbMax;
+ }
+
+ inline void getNonvirtualAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax, btScalar margin) const
+ {
+
+ //lazy evaluation of local aabb
+ btAssert(m_isLocalAabbValid);
+ btTransformAabb(m_localAabbMin,m_localAabbMax,margin,trans,aabbMin,aabbMax);
+ }
+
+public:
+
+ virtual void setLocalScaling(const btVector3& scaling);
+
+ virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
+
+ void recalcLocalAabb();
+
+};
+
+#endif //BT_CONVEX_INTERNAL_SHAPE_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btConvexPointCloudShape.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btConvexPointCloudShape.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btConvexPointCloudShape.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btConvexPointCloudShape.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,105 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_CONVEX_POINT_CLOUD_SHAPE_H
+#define BT_CONVEX_POINT_CLOUD_SHAPE_H
+
+#include "btPolyhedralConvexShape.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
+#include "LinearMath/btAlignedObjectArray.h"
+
+///The btConvexPointCloudShape implements an implicit convex hull of an array of vertices.
+ATTRIBUTE_ALIGNED16(class) btConvexPointCloudShape : public btPolyhedralConvexAabbCachingShape
+{
+ btVector3* m_unscaledPoints;
+ int m_numPoints;
+
+public:
+ BT_DECLARE_ALIGNED_ALLOCATOR();
+
+ btConvexPointCloudShape()
+ {
+ m_localScaling.setValue(1.f,1.f,1.f);
+ m_shapeType = CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE;
+ m_unscaledPoints = 0;
+ m_numPoints = 0;
+ }
+
+ btConvexPointCloudShape(btVector3* points,int numPoints, const btVector3& localScaling,bool computeAabb = true)
+ {
+ m_localScaling = localScaling;
+ m_shapeType = CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE;
+ m_unscaledPoints = points;
+ m_numPoints = numPoints;
+
+ if (computeAabb)
+ recalcLocalAabb();
+ }
+
+ void setPoints (btVector3* points, int numPoints, bool computeAabb = true,const btVector3& localScaling=btVector3(1.f,1.f,1.f))
+ {
+ m_unscaledPoints = points;
+ m_numPoints = numPoints;
+ m_localScaling = localScaling;
+
+ if (computeAabb)
+ recalcLocalAabb();
+ }
+
+ SIMD_FORCE_INLINE btVector3* getUnscaledPoints()
+ {
+ return m_unscaledPoints;
+ }
+
+ SIMD_FORCE_INLINE const btVector3* getUnscaledPoints() const
+ {
+ return m_unscaledPoints;
+ }
+
+ SIMD_FORCE_INLINE int getNumPoints() const
+ {
+ return m_numPoints;
+ }
+
+ SIMD_FORCE_INLINE btVector3 getScaledPoint( int index) const
+ {
+ return m_unscaledPoints[index] * m_localScaling;
+ }
+
+#ifndef __SPU__
+ virtual btVector3 localGetSupportingVertex(const btVector3& vec)const;
+ virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
+ virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
+#endif
+
+
+ //debugging
+ virtual const char* getName()const {return "ConvexPointCloud";}
+
+ virtual int getNumVertices() const;
+ virtual int getNumEdges() const;
+ virtual void getEdge(int i,btVector3& pa,btVector3& pb) const;
+ virtual void getVertex(int i,btVector3& vtx) const;
+ virtual int getNumPlanes() const;
+ virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const;
+ virtual bool isInside(const btVector3& pt,btScalar tolerance) const;
+
+ ///in case we receive negative scaling
+ virtual void setLocalScaling(const btVector3& scaling);
+};
+
+
+#endif //BT_CONVEX_POINT_CLOUD_SHAPE_H
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btConvexShape.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btConvexShape.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btConvexShape.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btConvexShape.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,82 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef CONVEX_SHAPE_INTERFACE1
+#define CONVEX_SHAPE_INTERFACE1
+
+#include "btCollisionShape.h"
+
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btMatrix3x3.h"
+#include "btCollisionMargin.h"
+#include "LinearMath/btAlignedAllocator.h"
+
+#define MAX_PREFERRED_PENETRATION_DIRECTIONS 10
+
+/// The btConvexShape is an abstract shape interface, implemented by all convex shapes such as btBoxShape, btConvexHullShape etc.
+/// It describes general convex shapes using the localGetSupportingVertex interface, used by collision detectors such as btGjkPairDetector.
+ATTRIBUTE_ALIGNED16(class) btConvexShape : public btCollisionShape
+{
+
+
+public:
+
+ BT_DECLARE_ALIGNED_ALLOCATOR();
+
+ btConvexShape ();
+
+ virtual ~btConvexShape();
+
+ virtual btVector3 localGetSupportingVertex(const btVector3& vec)const = 0;
+
+ ////////
+ #ifndef __SPU__
+ virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const=0;
+ #endif //#ifndef __SPU__
+
+ btVector3 localGetSupportVertexWithoutMarginNonVirtual (const btVector3& vec) const;
+ btVector3 localGetSupportVertexNonVirtual (const btVector3& vec) const;
+ btScalar getMarginNonVirtual () const;
+ void getAabbNonVirtual (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const;
+
+
+ //notice that the vectors should be unit length
+ virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const= 0;
+
+ ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
+ void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const =0;
+
+ virtual void getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const =0;
+
+ virtual void setLocalScaling(const btVector3& scaling) =0;
+ virtual const btVector3& getLocalScaling() const =0;
+
+ virtual void setMargin(btScalar margin)=0;
+
+ virtual btScalar getMargin() const=0;
+
+ virtual int getNumPreferredPenetrationDirections() const=0;
+
+ virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const=0;
+
+
+
+
+};
+
+
+
+#endif //CONVEX_SHAPE_INTERFACE1
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,75 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+#ifndef CONVEX_TRIANGLEMESH_SHAPE_H
+#define CONVEX_TRIANGLEMESH_SHAPE_H
+
+
+#include "btPolyhedralConvexShape.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
+
+
+/// The btConvexTriangleMeshShape is a convex hull of a triangle mesh, but the performance is not as good as btConvexHullShape.
+/// A small benefit of this class is that it uses the btStridingMeshInterface, so you can avoid the duplication of the triangle mesh data. Nevertheless, most users should use the much better performing btConvexHullShape instead.
+class btConvexTriangleMeshShape : public btPolyhedralConvexAabbCachingShape
+{
+
+ class btStridingMeshInterface* m_stridingMesh;
+
+public:
+ btConvexTriangleMeshShape(btStridingMeshInterface* meshInterface, bool calcAabb = true);
+
+ class btStridingMeshInterface* getMeshInterface()
+ {
+ return m_stridingMesh;
+ }
+ const class btStridingMeshInterface* getMeshInterface() const
+ {
+ return m_stridingMesh;
+ }
+
+ virtual btVector3 localGetSupportingVertex(const btVector3& vec)const;
+ virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
+ virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
+
+ //debugging
+ virtual const char* getName()const {return "ConvexTrimesh";}
+
+ virtual int getNumVertices() const;
+ virtual int getNumEdges() const;
+ virtual void getEdge(int i,btVector3& pa,btVector3& pb) const;
+ virtual void getVertex(int i,btVector3& vtx) const;
+ virtual int getNumPlanes() const;
+ virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const;
+ virtual bool isInside(const btVector3& pt,btScalar tolerance) const;
+
+
+ virtual void setLocalScaling(const btVector3& scaling);
+ virtual const btVector3& getLocalScaling() const;
+
+ ///computes the exact moment of inertia and the transform from the coordinate system defined by the principal axes of the moment of inertia
+ ///and the center of mass to the current coordinate system. A mass of 1 is assumed, for other masses just multiply the computed "inertia"
+ ///by the mass. The resulting transform "principal" has to be applied inversely to the mesh in order for the local coordinate system of the
+ ///shape to be centered at the center of mass and to coincide with the principal axes. This also necessitates a correction of the world transform
+ ///of the collision object by the principal transform. This method also computes the volume of the convex mesh.
+ void calculatePrincipalAxisTransform(btTransform& principal, btVector3& inertia, btScalar& volume) const;
+
+};
+
+
+
+#endif //CONVEX_TRIANGLEMESH_SHAPE_H
+
+
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btCylinderShape.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btCylinderShape.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btCylinderShape.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btCylinderShape.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,161 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef CYLINDER_MINKOWSKI_H
+#define CYLINDER_MINKOWSKI_H
+
+#include "btBoxShape.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
+#include "LinearMath/btVector3.h"
+
+/// The btCylinderShape class implements a cylinder shape primitive, centered around the origin. Its central axis aligned with the Y axis. btCylinderShapeX is aligned with the X axis and btCylinderShapeZ around the Z axis.
+class btCylinderShape : public btConvexInternalShape
+
+{
+
+protected:
+
+ int m_upAxis;
+
+public:
+
+ btVector3 getHalfExtentsWithMargin() const
+ {
+ btVector3 halfExtents = getHalfExtentsWithoutMargin();
+ btVector3 margin(getMargin(),getMargin(),getMargin());
+ halfExtents += margin;
+ return halfExtents;
+ }
+
+ const btVector3& getHalfExtentsWithoutMargin() const
+ {
+ return m_implicitShapeDimensions;//changed in Bullet 2.63: assume the scaling and margin are included
+ }
+
+ btCylinderShape (const btVector3& halfExtents);
+
+ void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
+
+ virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+
+ virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
+
+ virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
+
+ virtual void setMargin(btScalar collisionMargin)
+ {
+ //correct the m_implicitShapeDimensions for the margin
+ btVector3 oldMargin(getMargin(),getMargin(),getMargin());
+ btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin;
+
+ btConvexInternalShape::setMargin(collisionMargin);
+ btVector3 newMargin(getMargin(),getMargin(),getMargin());
+ m_implicitShapeDimensions = implicitShapeDimensionsWithMargin - newMargin;
+
+ }
+
+ virtual btVector3 localGetSupportingVertex(const btVector3& vec) const
+ {
+
+ btVector3 supVertex;
+ supVertex = localGetSupportingVertexWithoutMargin(vec);
+
+ if ( getMargin()!=btScalar(0.) )
+ {
+ btVector3 vecnorm = vec;
+ if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON))
+ {
+ vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.));
+ }
+ vecnorm.normalize();
+ supVertex+= getMargin() * vecnorm;
+ }
+ return supVertex;
+ }
+
+
+ //use box inertia
+ // virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+
+
+ int getUpAxis() const
+ {
+ return m_upAxis;
+ }
+
+ virtual btScalar getRadius() const
+ {
+ return getHalfExtentsWithMargin().getX();
+ }
+
+ //debugging
+ virtual const char* getName()const
+ {
+ return "CylinderY";
+ }
+
+
+
+};
+
+class btCylinderShapeX : public btCylinderShape
+{
+public:
+ btCylinderShapeX (const btVector3& halfExtents);
+
+ virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
+ virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
+
+ //debugging
+ virtual const char* getName()const
+ {
+ return "CylinderX";
+ }
+
+ virtual btScalar getRadius() const
+ {
+ return getHalfExtentsWithMargin().getY();
+ }
+
+};
+
+class btCylinderShapeZ : public btCylinderShape
+{
+public:
+ btCylinderShapeZ (const btVector3& halfExtents);
+
+ virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
+ virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
+
+ virtual int getUpAxis() const
+ {
+ return 2;
+ }
+ //debugging
+ virtual const char* getName()const
+ {
+ return "CylinderZ";
+ }
+
+ virtual btScalar getRadius() const
+ {
+ return getHalfExtentsWithMargin().getX();
+ }
+
+};
+
+
+#endif //CYLINDER_MINKOWSKI_H
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btEmptyShape.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btEmptyShape.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btEmptyShape.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btEmptyShape.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,70 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef EMPTY_SHAPE_H
+#define EMPTY_SHAPE_H
+
+#include "btConcaveShape.h"
+
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btMatrix3x3.h"
+#include "btCollisionMargin.h"
+
+
+
+
+/// The btEmptyShape is a collision shape without actual collision detection shape, so most users should ignore this class.
+/// It can be replaced by another shape during runtime, but the inertia tensor should be recomputed.
+class btEmptyShape : public btConcaveShape
+{
+public:
+ btEmptyShape();
+
+ virtual ~btEmptyShape();
+
+
+ ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
+ void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
+
+
+ virtual void setLocalScaling(const btVector3& scaling)
+ {
+ m_localScaling = scaling;
+ }
+ virtual const btVector3& getLocalScaling() const
+ {
+ return m_localScaling;
+ }
+
+ virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+
+ virtual const char* getName()const
+ {
+ return "Empty";
+ }
+
+ virtual void processAllTriangles(btTriangleCallback* ,const btVector3& ,const btVector3& ) const
+ {
+ }
+
+protected:
+ btVector3 m_localScaling;
+
+};
+
+
+
+#endif //EMPTY_SHAPE_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,161 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef HEIGHTFIELD_TERRAIN_SHAPE_H
+#define HEIGHTFIELD_TERRAIN_SHAPE_H
+
+#include "btConcaveShape.h"
+
+///btHeightfieldTerrainShape simulates a 2D heightfield terrain
+/**
+ The caller is responsible for maintaining the heightfield array; this
+ class does not make a copy.
+
+ The heightfield can be dynamic so long as the min/max height values
+ capture the extremes (heights must always be in that range).
+
+ The local origin of the heightfield is assumed to be the exact
+ center (as determined by width and length and height, with each
+ axis multiplied by the localScaling).
+
+ \b NOTE: be careful with coordinates. If you have a heightfield with a local
+ min height of -100m, and a max height of +500m, you may be tempted to place it
+ at the origin (0,0) and expect the heights in world coordinates to be
+ -100 to +500 meters.
+ Actually, the heights will be -300 to +300m, because bullet will re-center
+ the heightfield based on its AABB (which is determined by the min/max
+ heights). So keep in mind that once you create a btHeightfieldTerrainShape
+ object, the heights will be adjusted relative to the center of the AABB. This
+ is different to the behavior of many rendering engines, but is useful for
+ physics engines.
+
+ Most (but not all) rendering and heightfield libraries assume upAxis = 1
+ (that is, the y-axis is "up"). This class allows any of the 3 coordinates
+ to be "up". Make sure your choice of axis is consistent with your rendering
+ system.
+
+ The heightfield heights are determined from the data type used for the
+ heightfieldData array.
+
+ - PHY_UCHAR: height at a point is the uchar value at the
+ grid point, multipled by heightScale. uchar isn't recommended
+ because of its inability to deal with negative values, and
+ low resolution (8-bit).
+
+ - PHY_SHORT: height at a point is the short int value at that grid
+ point, multipled by heightScale.
+
+ - PHY_FLOAT: height at a point is the float value at that grid
+ point. heightScale is ignored when using the float heightfield
+ data type.
+
+ Whatever the caller specifies as minHeight and maxHeight will be honored.
+ The class will not inspect the heightfield to discover the actual minimum
+ or maximum heights. These values are used to determine the heightfield's
+ axis-aligned bounding box, multiplied by localScaling.
+
+ For usage and testing see the TerrainDemo.
+ */
+class btHeightfieldTerrainShape : public btConcaveShape
+{
+protected:
+ btVector3 m_localAabbMin;
+ btVector3 m_localAabbMax;
+ btVector3 m_localOrigin;
+
+ ///terrain data
+ int m_heightStickWidth;
+ int m_heightStickLength;
+ btScalar m_minHeight;
+ btScalar m_maxHeight;
+ btScalar m_width;
+ btScalar m_length;
+ btScalar m_heightScale;
+ union
+ {
+ unsigned char* m_heightfieldDataUnsignedChar;
+ short* m_heightfieldDataShort;
+ btScalar* m_heightfieldDataFloat;
+ void* m_heightfieldDataUnknown;
+ };
+
+ PHY_ScalarType m_heightDataType;
+ bool m_flipQuadEdges;
+ bool m_useDiamondSubdivision;
+
+ int m_upAxis;
+
+ btVector3 m_localScaling;
+
+ virtual btScalar getRawHeightFieldValue(int x,int y) const;
+ void quantizeWithClamp(int* out, const btVector3& point,int isMax) const;
+ void getVertex(int x,int y,btVector3& vertex) const;
+
+
+
+ /// protected initialization
+ /**
+ Handles the work of constructors so that public constructors can be
+ backwards-compatible without a lot of copy/paste.
+ */
+ void initialize(int heightStickWidth, int heightStickLength,
+ void* heightfieldData, btScalar heightScale,
+ btScalar minHeight, btScalar maxHeight, int upAxis,
+ PHY_ScalarType heightDataType, bool flipQuadEdges);
+
+public:
+ /// preferred constructor
+ /**
+ This constructor supports a range of heightfield
+ data types, and allows for a non-zero minimum height value.
+ heightScale is needed for any integer-based heightfield data types.
+ */
+ btHeightfieldTerrainShape(int heightStickWidth,int heightStickLength,
+ void* heightfieldData, btScalar heightScale,
+ btScalar minHeight, btScalar maxHeight,
+ int upAxis, PHY_ScalarType heightDataType,
+ bool flipQuadEdges);
+
+ /// legacy constructor
+ /**
+ The legacy constructor assumes the heightfield has a minimum height
+ of zero. Only unsigned char or floats are supported. For legacy
+ compatibility reasons, heightScale is calculated as maxHeight / 65535
+ (and is only used when useFloatData = false).
+ */
+ btHeightfieldTerrainShape(int heightStickWidth,int heightStickLength,void* heightfieldData, btScalar maxHeight,int upAxis,bool useFloatData,bool flipQuadEdges);
+
+ virtual ~btHeightfieldTerrainShape();
+
+
+ void setUseDiamondSubdivision(bool useDiamondSubdivision=true) { m_useDiamondSubdivision = useDiamondSubdivision;}
+
+
+ virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
+
+ virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const;
+
+ virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+
+ virtual void setLocalScaling(const btVector3& scaling);
+
+ virtual const btVector3& getLocalScaling() const;
+
+ //debugging
+ virtual const char* getName()const {return "HEIGHTFIELD";}
+
+};
+
+#endif //HEIGHTFIELD_TERRAIN_SHAPE_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btMaterial.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btMaterial.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btMaterial.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btMaterial.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,34 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+/// This file was created by Alex Silverman
+
+#ifndef MATERIAL_H
+#define MATERIAL_H
+
+// Material class to be used by btMultimaterialTriangleMeshShape to store triangle properties
+class btMaterial
+{
+ // public members so that materials can change due to world events
+public:
+ btScalar m_friction;
+ btScalar m_restitution;
+ int pad[2];
+
+ btMaterial(){}
+ btMaterial(btScalar fric, btScalar rest) { m_friction = fric; m_restitution = rest; }
+};
+
+#endif // MATERIAL_H
\ No newline at end of file
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btMinkowskiSumShape.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btMinkowskiSumShape.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btMinkowskiSumShape.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btMinkowskiSumShape.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,60 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef MINKOWSKI_SUM_SHAPE_H
+#define MINKOWSKI_SUM_SHAPE_H
+
+#include "btConvexInternalShape.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
+
+/// The btMinkowskiSumShape is only for advanced users. This shape represents implicit based minkowski sum of two convex implicit shapes.
+class btMinkowskiSumShape : public btConvexInternalShape
+{
+
+ btTransform m_transA;
+ btTransform m_transB;
+ const btConvexShape* m_shapeA;
+ const btConvexShape* m_shapeB;
+
+public:
+
+ btMinkowskiSumShape(const btConvexShape* shapeA,const btConvexShape* shapeB);
+
+ virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
+
+ virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
+
+
+ virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+
+ void setTransformA(const btTransform& transA) { m_transA = transA;}
+ void setTransformB(const btTransform& transB) { m_transB = transB;}
+
+ const btTransform& getTransformA()const { return m_transA;}
+ const btTransform& GetTransformB()const { return m_transB;}
+
+
+ virtual btScalar getMargin() const;
+
+ const btConvexShape* getShapeA() const { return m_shapeA;}
+ const btConvexShape* getShapeB() const { return m_shapeB;}
+
+ virtual const char* getName()const
+ {
+ return "MinkowskiSum";
+ }
+};
+
+#endif //MINKOWSKI_SUM_SHAPE_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btMultiSphereShape.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btMultiSphereShape.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btMultiSphereShape.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btMultiSphereShape.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,68 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef MULTI_SPHERE_MINKOWSKI_H
+#define MULTI_SPHERE_MINKOWSKI_H
+
+#include "btConvexInternalShape.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
+#include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btAabbUtil2.h"
+
+///The btMultiSphereShape represents the convex hull of a collection of spheres. You can create special capsules or other smooth volumes.
+///It is possible to animate the spheres for deformation, but call 'recalcLocalAabb' after changing any sphere position/radius
+class btMultiSphereShape : public btConvexInternalAabbCachingShape
+{
+
+ btAlignedObjectArray<btVector3> m_localPositionArray;
+ btAlignedObjectArray<btScalar> m_radiArray;
+
+public:
+ btMultiSphereShape (const btVector3* positions,const btScalar* radi,int numSpheres);
+
+ ///CollisionShape Interface
+ virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+
+ /// btConvexShape Interface
+ virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
+
+ virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
+
+ int getSphereCount() const
+ {
+ return m_localPositionArray.size();
+ }
+
+ const btVector3& getSpherePosition(int index) const
+ {
+ return m_localPositionArray[index];
+ }
+
+ btScalar getSphereRadius(int index) const
+ {
+ return m_radiArray[index];
+ }
+
+
+ virtual const char* getName()const
+ {
+ return "MultiSphere";
+ }
+
+
+};
+
+
+#endif //MULTI_SPHERE_MINKOWSKI_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,123 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+/// This file was created by Alex Silverman
+
+#ifndef BVH_TRIANGLE_MATERIAL_MESH_SHAPE_H
+#define BVH_TRIANGLE_MATERIAL_MESH_SHAPE_H
+
+#include "btBvhTriangleMeshShape.h"
+#include "btMaterial.h"
+
+///The BvhTriangleMaterialMeshShape extends the btBvhTriangleMeshShape. Its main contribution is the interface into a material array, which allows per-triangle friction and restitution.
+ATTRIBUTE_ALIGNED16(class) btMultimaterialTriangleMeshShape : public btBvhTriangleMeshShape
+{
+ btAlignedObjectArray <btMaterial*> m_materialList;
+ int ** m_triangleMaterials;
+
+public:
+
+ BT_DECLARE_ALIGNED_ALLOCATOR();
+
+ btMultimaterialTriangleMeshShape(): btBvhTriangleMeshShape() {m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE;}
+ btMultimaterialTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression, bool buildBvh = true):
+ btBvhTriangleMeshShape(meshInterface, useQuantizedAabbCompression, buildBvh)
+ {
+ m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE;
+
+ btVector3 m_triangle[3];
+ const unsigned char *vertexbase;
+ int numverts;
+ PHY_ScalarType type;
+ int stride;
+ const unsigned char *indexbase;
+ int indexstride;
+ int numfaces;
+ PHY_ScalarType indicestype;
+
+ //m_materialLookup = (int**)(btAlignedAlloc(sizeof(int*) * meshInterface->getNumSubParts(), 16));
+
+ for(int i = 0; i < meshInterface->getNumSubParts(); i++)
+ {
+ m_meshInterface->getLockedReadOnlyVertexIndexBase(
+ &vertexbase,
+ numverts,
+ type,
+ stride,
+ &indexbase,
+ indexstride,
+ numfaces,
+ indicestype,
+ i);
+ //m_materialLookup[i] = (int*)(btAlignedAlloc(sizeof(int) * numfaces, 16));
+ }
+ }
+
+ ///optionally pass in a larger bvh aabb, used for quantization. This allows for deformations within this aabb
+ btMultimaterialTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression,const btVector3& bvhAabbMin,const btVector3& bvhAabbMax, bool buildBvh = true):
+ btBvhTriangleMeshShape(meshInterface, useQuantizedAabbCompression, bvhAabbMin, bvhAabbMax, buildBvh)
+ {
+ m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE;
+
+ btVector3 m_triangle[3];
+ const unsigned char *vertexbase;
+ int numverts;
+ PHY_ScalarType type;
+ int stride;
+ const unsigned char *indexbase;
+ int indexstride;
+ int numfaces;
+ PHY_ScalarType indicestype;
+
+ //m_materialLookup = (int**)(btAlignedAlloc(sizeof(int*) * meshInterface->getNumSubParts(), 16));
+
+ for(int i = 0; i < meshInterface->getNumSubParts(); i++)
+ {
+ m_meshInterface->getLockedReadOnlyVertexIndexBase(
+ &vertexbase,
+ numverts,
+ type,
+ stride,
+ &indexbase,
+ indexstride,
+ numfaces,
+ indicestype,
+ i);
+ //m_materialLookup[i] = (int*)(btAlignedAlloc(sizeof(int) * numfaces * 2, 16));
+ }
+ }
+
+ virtual ~btMultimaterialTriangleMeshShape()
+ {
+/*
+ for(int i = 0; i < m_meshInterface->getNumSubParts(); i++)
+ {
+ btAlignedFree(m_materialValues[i]);
+ m_materialLookup[i] = NULL;
+ }
+ btAlignedFree(m_materialValues);
+ m_materialLookup = NULL;
+*/
+ }
+ //debugging
+ virtual const char* getName()const {return "MULTIMATERIALTRIANGLEMESH";}
+
+ ///Obtains the material for a specific triangle
+ const btMaterial * getMaterialProperties(int partID, int triIndex);
+
+}
+;
+
+#endif //BVH_TRIANGLE_MATERIAL_MESH_SHAPE_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btOptimizedBvh.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btOptimizedBvh.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btOptimizedBvh.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btOptimizedBvh.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,65 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///Contains contributions from Disney Studio's
+
+#ifndef OPTIMIZED_BVH_H
+#define OPTIMIZED_BVH_H
+
+#include "BulletCollision/BroadphaseCollision/btQuantizedBvh.h"
+
+class btStridingMeshInterface;
+
+
+///The btOptimizedBvh extends the btQuantizedBvh to create AABB tree for triangle meshes, through the btStridingMeshInterface.
+ATTRIBUTE_ALIGNED16(class) btOptimizedBvh : public btQuantizedBvh
+{
+
+public:
+ BT_DECLARE_ALIGNED_ALLOCATOR();
+
+protected:
+
+public:
+
+ btOptimizedBvh();
+
+ virtual ~btOptimizedBvh();
+
+ void build(btStridingMeshInterface* triangles,bool useQuantizedAabbCompression, const btVector3& bvhAabbMin, const btVector3& bvhAabbMax);
+
+ void refit(btStridingMeshInterface* triangles,const btVector3& aabbMin,const btVector3& aabbMax);
+
+ void refitPartial(btStridingMeshInterface* triangles,const btVector3& aabbMin, const btVector3& aabbMax);
+
+ void updateBvhNodes(btStridingMeshInterface* meshInterface,int firstNode,int endNode,int index);
+
+ /// Data buffer MUST be 16 byte aligned
+ virtual bool serialize(void *o_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian)
+ {
+ return btQuantizedBvh::serialize(o_alignedDataBuffer,i_dataBufferSize,i_swapEndian);
+
+ }
+
+ ///deSerializeInPlace loads and initializes a BVH from a buffer in memory 'in place'
+ static btOptimizedBvh *deSerializeInPlace(void *i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian);
+
+
+};
+
+
+#endif //OPTIMIZED_BVH_H
+
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,98 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BU_SHAPE
+#define BU_SHAPE
+
+#include "LinearMath/btMatrix3x3.h"
+#include "btConvexInternalShape.h"
+
+
+///The btPolyhedralConvexShape is an internal interface class for polyhedral convex shapes.
+class btPolyhedralConvexShape : public btConvexInternalShape
+{
+
+protected:
+
+public:
+
+ btPolyhedralConvexShape();
+
+ //brute force implementations
+
+ virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
+ virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
+
+ virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+
+
+ virtual int getNumVertices() const = 0 ;
+ virtual int getNumEdges() const = 0;
+ virtual void getEdge(int i,btVector3& pa,btVector3& pb) const = 0;
+ virtual void getVertex(int i,btVector3& vtx) const = 0;
+ virtual int getNumPlanes() const = 0;
+ virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const = 0;
+// virtual int getIndex(int i) const = 0 ;
+
+ virtual bool isInside(const btVector3& pt,btScalar tolerance) const = 0;
+
+};
+
+
+///The btPolyhedralConvexAabbCachingShape adds aabb caching to the btPolyhedralConvexShape
+class btPolyhedralConvexAabbCachingShape : public btPolyhedralConvexShape
+{
+
+ btVector3 m_localAabbMin;
+ btVector3 m_localAabbMax;
+ bool m_isLocalAabbValid;
+
+protected:
+
+ void setCachedLocalAabb (const btVector3& aabbMin, const btVector3& aabbMax)
+ {
+ m_isLocalAabbValid = true;
+ m_localAabbMin = aabbMin;
+ m_localAabbMax = aabbMax;
+ }
+
+ inline void getCachedLocalAabb (btVector3& aabbMin, btVector3& aabbMax) const
+ {
+ btAssert(m_isLocalAabbValid);
+ aabbMin = m_localAabbMin;
+ aabbMax = m_localAabbMax;
+ }
+
+public:
+
+ btPolyhedralConvexAabbCachingShape();
+
+ inline void getNonvirtualAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax, btScalar margin) const
+ {
+
+ //lazy evaluation of local aabb
+ btAssert(m_isLocalAabbValid);
+ btTransformAabb(m_localAabbMin,m_localAabbMax,margin,trans,aabbMin,aabbMax);
+ }
+
+ virtual void setLocalScaling(const btVector3& scaling);
+
+ virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
+
+ void recalcLocalAabb();
+
+};
+
+#endif //BU_SHAPE
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,62 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef SCALED_BVH_TRIANGLE_MESH_SHAPE_H
+#define SCALED_BVH_TRIANGLE_MESH_SHAPE_H
+
+#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
+
+
+///The btScaledBvhTriangleMeshShape allows to instance a scaled version of an existing btBvhTriangleMeshShape.
+///Note that each btBvhTriangleMeshShape still can have its own local scaling, independent from this btScaledBvhTriangleMeshShape 'localScaling'
+ATTRIBUTE_ALIGNED16(class) btScaledBvhTriangleMeshShape : public btConcaveShape
+{
+
+
+ btVector3 m_localScaling;
+
+ btBvhTriangleMeshShape* m_bvhTriMeshShape;
+
+public:
+
+
+ btScaledBvhTriangleMeshShape(btBvhTriangleMeshShape* childShape,const btVector3& localScaling);
+
+ virtual ~btScaledBvhTriangleMeshShape();
+
+
+ virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
+ virtual void setLocalScaling(const btVector3& scaling);
+ virtual const btVector3& getLocalScaling() const;
+ virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+
+ virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const;
+
+ btBvhTriangleMeshShape* getChildShape()
+ {
+ return m_bvhTriMeshShape;
+ }
+
+ const btBvhTriangleMeshShape* getChildShape() const
+ {
+ return m_bvhTriMeshShape;
+ }
+
+ //debugging
+ virtual const char* getName()const {return "SCALEDBVHTRIANGLEMESH";}
+
+};
+
+#endif //BVH_TRIANGLE_MESH_SHAPE_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btShapeHull.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btShapeHull.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btShapeHull.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btShapeHull.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,56 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///btShapeHull implemented by John McCutchan.
+
+#ifndef _SHAPE_HULL_H
+#define _SHAPE_HULL_H
+
+#include "LinearMath/btAlignedObjectArray.h"
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+
+
+///The btShapeHull class takes a btConvexShape, builds a simplified convex hull using btConvexHull and provides triangle indices and vertices.
+///It can be useful for to simplify a complex convex object and for visualization of a non-polyhedral convex object.
+///It approximates the convex hull using the supporting vertex of 42 directions.
+class btShapeHull
+{
+public:
+ btShapeHull (const btConvexShape* shape);
+ ~btShapeHull ();
+
+ bool buildHull (btScalar margin);
+
+ int numTriangles () const;
+ int numVertices () const;
+ int numIndices () const;
+
+ const btVector3* getVertexPointer() const
+ {
+ return &m_vertices[0];
+ }
+ const unsigned int* getIndexPointer() const
+ {
+ return &m_indices[0];
+ }
+
+protected:
+ btAlignedObjectArray<btVector3> m_vertices;
+ btAlignedObjectArray<unsigned int> m_indices;
+ unsigned int m_numIndices;
+ const btConvexShape* m_shape;
+};
+
+#endif //_SHAPE_HULL_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btSphereShape.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btSphereShape.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btSphereShape.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btSphereShape.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,73 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+#ifndef SPHERE_MINKOWSKI_H
+#define SPHERE_MINKOWSKI_H
+
+#include "btConvexInternalShape.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
+
+///The btSphereShape implements an implicit sphere, centered around a local origin with radius.
+ATTRIBUTE_ALIGNED16(class) btSphereShape : public btConvexInternalShape
+
+{
+
+public:
+ BT_DECLARE_ALIGNED_ALLOCATOR();
+
+ btSphereShape (btScalar radius) : btConvexInternalShape ()
+ {
+ m_shapeType = SPHERE_SHAPE_PROXYTYPE;
+ m_implicitShapeDimensions.setX(radius);
+ m_collisionMargin = radius;
+ }
+
+ virtual btVector3 localGetSupportingVertex(const btVector3& vec)const;
+ virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
+ //notice that the vectors should be unit length
+ virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
+
+
+ virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+
+ virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
+
+
+ btScalar getRadius() const { return m_implicitShapeDimensions.getX() * m_localScaling.getX();}
+
+ void setUnscaledRadius(btScalar radius)
+ {
+ m_implicitShapeDimensions.setX(radius);
+ btConvexInternalShape::setMargin(radius);
+ }
+
+ //debugging
+ virtual const char* getName()const {return "SPHERE";}
+
+ virtual void setMargin(btScalar margin)
+ {
+ btConvexInternalShape::setMargin(margin);
+ }
+ virtual btScalar getMargin() const
+ {
+ //to improve gjk behaviour, use radius+margin as the full margin, so never get into the penetration case
+ //this means, non-uniform scaling is not supported anymore
+ return getRadius();
+ }
+
+
+};
+
+
+#endif //SPHERE_MINKOWSKI_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btStaticPlaneShape.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btStaticPlaneShape.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btStaticPlaneShape.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btStaticPlaneShape.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,64 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef STATIC_PLANE_SHAPE_H
+#define STATIC_PLANE_SHAPE_H
+
+#include "btConcaveShape.h"
+
+
+///The btStaticPlaneShape simulates an infinite non-moving (static) collision plane.
+class btStaticPlaneShape : public btConcaveShape
+{
+protected:
+ btVector3 m_localAabbMin;
+ btVector3 m_localAabbMax;
+
+ btVector3 m_planeNormal;
+ btScalar m_planeConstant;
+ btVector3 m_localScaling;
+
+public:
+ btStaticPlaneShape(const btVector3& planeNormal,btScalar planeConstant);
+
+ virtual ~btStaticPlaneShape();
+
+
+ virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
+
+ virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const;
+
+ virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+
+ virtual void setLocalScaling(const btVector3& scaling);
+ virtual const btVector3& getLocalScaling() const;
+
+ const btVector3& getPlaneNormal() const
+ {
+ return m_planeNormal;
+ }
+
+ const btScalar& getPlaneConstant() const
+ {
+ return m_planeConstant;
+ }
+
+ //debugging
+ virtual const char* getName()const {return "STATICPLANE";}
+
+
+};
+
+#endif //STATIC_PLANE_SHAPE_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btStridingMeshInterface.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btStridingMeshInterface.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btStridingMeshInterface.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btStridingMeshInterface.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,96 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef STRIDING_MESHINTERFACE_H
+#define STRIDING_MESHINTERFACE_H
+
+#include "LinearMath/btVector3.h"
+#include "btTriangleCallback.h"
+#include "btConcaveShape.h"
+
+
+
+/// The btStridingMeshInterface is the interface class for high performance generic access to triangle meshes, used in combination with btBvhTriangleMeshShape and some other collision shapes.
+/// Using index striding of 3*sizeof(integer) it can use triangle arrays, using index striding of 1*sizeof(integer) it can handle triangle strips.
+/// It allows for sharing graphics and collision meshes. Also it provides locking/unlocking of graphics meshes that are in gpu memory.
+class btStridingMeshInterface
+{
+ protected:
+
+ btVector3 m_scaling;
+
+ public:
+ btStridingMeshInterface() :m_scaling(btScalar(1.),btScalar(1.),btScalar(1.))
+ {
+
+ }
+
+ virtual ~btStridingMeshInterface();
+
+
+
+ virtual void InternalProcessAllTriangles(btInternalTriangleIndexCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const;
+
+ ///brute force method to calculate aabb
+ void calculateAabbBruteForce(btVector3& aabbMin,btVector3& aabbMax);
+
+ /// get read and write access to a subpart of a triangle mesh
+ /// this subpart has a continuous array of vertices and indices
+ /// in this way the mesh can be handled as chunks of memory with striding
+ /// very similar to OpenGL vertexarray support
+ /// make a call to unLockVertexBase when the read and write access is finished
+ virtual void getLockedVertexIndexBase(unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& stride,unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart=0)=0;
+
+ virtual void getLockedReadOnlyVertexIndexBase(const unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& stride,const unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart=0) const=0;
+
+ /// unLockVertexBase finishes the access to a subpart of the triangle mesh
+ /// make a call to unLockVertexBase when the read and write access (using getLockedVertexIndexBase) is finished
+ virtual void unLockVertexBase(int subpart)=0;
+
+ virtual void unLockReadOnlyVertexBase(int subpart) const=0;
+
+
+ /// getNumSubParts returns the number of seperate subparts
+ /// each subpart has a continuous array of vertices and indices
+ virtual int getNumSubParts() const=0;
+
+ virtual void preallocateVertices(int numverts)=0;
+ virtual void preallocateIndices(int numindices)=0;
+
+ virtual bool hasPremadeAabb() const { return false; }
+ virtual void setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax ) const
+ {
+ (void) aabbMin;
+ (void) aabbMax;
+ }
+ virtual void getPremadeAabb(btVector3* aabbMin, btVector3* aabbMax ) const
+ {
+ (void) aabbMin;
+ (void) aabbMax;
+ }
+
+ const btVector3& getScaling() const {
+ return m_scaling;
+ }
+ void setScaling(const btVector3& scaling)
+ {
+ m_scaling = scaling;
+ }
+
+
+
+};
+
+#endif //STRIDING_MESHINTERFACE_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btTetrahedronShape.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btTetrahedronShape.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btTetrahedronShape.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btTetrahedronShape.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,74 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BU_SIMPLEX_1TO4_SHAPE
+#define BU_SIMPLEX_1TO4_SHAPE
+
+
+#include "btPolyhedralConvexShape.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+
+
+///The btBU_Simplex1to4 implements tetrahedron, triangle, line, vertex collision shapes. In most cases it is better to use btConvexHullShape instead.
+class btBU_Simplex1to4 : public btPolyhedralConvexAabbCachingShape
+{
+protected:
+
+ int m_numVertices;
+ btVector3 m_vertices[4];
+
+public:
+ btBU_Simplex1to4();
+
+ btBU_Simplex1to4(const btVector3& pt0);
+ btBU_Simplex1to4(const btVector3& pt0,const btVector3& pt1);
+ btBU_Simplex1to4(const btVector3& pt0,const btVector3& pt1,const btVector3& pt2);
+ btBU_Simplex1to4(const btVector3& pt0,const btVector3& pt1,const btVector3& pt2,const btVector3& pt3);
+
+
+ void reset()
+ {
+ m_numVertices = 0;
+ }
+
+ virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
+
+ void addVertex(const btVector3& pt);
+
+ //PolyhedralConvexShape interface
+
+ virtual int getNumVertices() const;
+
+ virtual int getNumEdges() const;
+
+ virtual void getEdge(int i,btVector3& pa,btVector3& pb) const;
+
+ virtual void getVertex(int i,btVector3& vtx) const;
+
+ virtual int getNumPlanes() const;
+
+ virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i) const;
+
+ virtual int getIndex(int i) const;
+
+ virtual bool isInside(const btVector3& pt,btScalar tolerance) const;
+
+
+ ///getName is for debugging
+ virtual const char* getName()const { return "btBU_Simplex1to4";}
+
+};
+
+#endif //BU_SIMPLEX_1TO4_SHAPE
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btTriangleBuffer.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btTriangleBuffer.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btTriangleBuffer.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btTriangleBuffer.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,69 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_TRIANGLE_BUFFER_H
+#define BT_TRIANGLE_BUFFER_H
+
+#include "btTriangleCallback.h"
+#include "LinearMath/btAlignedObjectArray.h"
+
+struct btTriangle
+{
+ btVector3 m_vertex0;
+ btVector3 m_vertex1;
+ btVector3 m_vertex2;
+ int m_partId;
+ int m_triangleIndex;
+};
+
+///The btTriangleBuffer callback can be useful to collect and store overlapping triangles between AABB and concave objects that support 'processAllTriangles'
+///Example usage of this class:
+/// btTriangleBuffer triBuf;
+/// concaveShape->processAllTriangles(&triBuf,aabbMin, aabbMax);
+/// for (int i=0;i<triBuf.getNumTriangles();i++)
+/// {
+/// const btTriangle& tri = triBuf.getTriangle(i);
+/// //do something useful here with the triangle
+/// }
+class btTriangleBuffer : public btTriangleCallback
+{
+
+ btAlignedObjectArray<btTriangle> m_triangleBuffer;
+
+public:
+
+
+ virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex);
+
+ int getNumTriangles() const
+ {
+ return int(m_triangleBuffer.size());
+ }
+
+ const btTriangle& getTriangle(int index) const
+ {
+ return m_triangleBuffer[index];
+ }
+
+ void clearBuffer()
+ {
+ m_triangleBuffer.clear();
+ }
+
+};
+
+
+#endif //BT_TRIANGLE_BUFFER_H
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btTriangleCallback.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btTriangleCallback.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btTriangleCallback.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btTriangleCallback.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,42 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef TRIANGLE_CALLBACK_H
+#define TRIANGLE_CALLBACK_H
+
+#include "LinearMath/btVector3.h"
+
+
+///The btTriangleCallback provides a callback for each overlapping triangle when calling processAllTriangles.
+///This callback is called by processAllTriangles for all btConcaveShape derived class, such as btBvhTriangleMeshShape, btStaticPlaneShape and btHeightfieldTerrainShape.
+class btTriangleCallback
+{
+public:
+
+ virtual ~btTriangleCallback();
+ virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) = 0;
+};
+
+class btInternalTriangleIndexCallback
+{
+public:
+
+ virtual ~btInternalTriangleIndexCallback();
+ virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) = 0;
+};
+
+
+
+#endif //TRIANGLE_CALLBACK_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,131 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_TRIANGLE_INDEX_VERTEX_ARRAY_H
+#define BT_TRIANGLE_INDEX_VERTEX_ARRAY_H
+
+#include "btStridingMeshInterface.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btScalar.h"
+
+
+///The btIndexedMesh indexes a single vertex and index array. Multiple btIndexedMesh objects can be passed into a btTriangleIndexVertexArray using addIndexedMesh.
+///Instead of the number of indices, we pass the number of triangles.
+ATTRIBUTE_ALIGNED16( struct) btIndexedMesh
+{
+ BT_DECLARE_ALIGNED_ALLOCATOR();
+
+ int m_numTriangles;
+ const unsigned char * m_triangleIndexBase;
+ int m_triangleIndexStride;
+ int m_numVertices;
+ const unsigned char * m_vertexBase;
+ int m_vertexStride;
+
+ // The index type is set when adding an indexed mesh to the
+ // btTriangleIndexVertexArray, do not set it manually
+ PHY_ScalarType m_indexType;
+
+ // The vertex type has a default type similar to Bullet's precision mode (float or double)
+ // but can be set manually if you for example run Bullet with double precision but have
+ // mesh data in single precision..
+ PHY_ScalarType m_vertexType;
+
+
+ btIndexedMesh()
+ :m_indexType(PHY_INTEGER),
+#ifdef BT_USE_DOUBLE_PRECISION
+ m_vertexType(PHY_DOUBLE)
+#else // BT_USE_DOUBLE_PRECISION
+ m_vertexType(PHY_FLOAT)
+#endif // BT_USE_DOUBLE_PRECISION
+ {
+ }
+}
+;
+
+
+typedef btAlignedObjectArray<btIndexedMesh> IndexedMeshArray;
+
+///The btTriangleIndexVertexArray allows to access multiple triangle meshes, by indexing into existing triangle/index arrays.
+///Additional meshes can be added using addIndexedMesh
+///No duplcate is made of the vertex/index data, it only indexes into external vertex/index arrays.
+///So keep those arrays around during the lifetime of this btTriangleIndexVertexArray.
+ATTRIBUTE_ALIGNED16( class) btTriangleIndexVertexArray : public btStridingMeshInterface
+{
+protected:
+ IndexedMeshArray m_indexedMeshes;
+ int m_pad[2];
+ mutable int m_hasAabb; // using int instead of bool to maintain alignment
+ mutable btVector3 m_aabbMin;
+ mutable btVector3 m_aabbMax;
+
+public:
+
+ BT_DECLARE_ALIGNED_ALLOCATOR();
+
+ btTriangleIndexVertexArray() : m_hasAabb(0)
+ {
+ }
+
+ virtual ~btTriangleIndexVertexArray();
+
+ //just to be backwards compatible
+ btTriangleIndexVertexArray(int numTriangles,int* triangleIndexBase,int triangleIndexStride,int numVertices,btScalar* vertexBase,int vertexStride);
+
+ void addIndexedMesh(const btIndexedMesh& mesh, PHY_ScalarType indexType = PHY_INTEGER)
+ {
+ m_indexedMeshes.push_back(mesh);
+ m_indexedMeshes[m_indexedMeshes.size()-1].m_indexType = indexType;
+ }
+
+
+ virtual void getLockedVertexIndexBase(unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& vertexStride,unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart=0);
+
+ virtual void getLockedReadOnlyVertexIndexBase(const unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& vertexStride,const unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart=0) const;
+
+ /// unLockVertexBase finishes the access to a subpart of the triangle mesh
+ /// make a call to unLockVertexBase when the read and write access (using getLockedVertexIndexBase) is finished
+ virtual void unLockVertexBase(int subpart) {(void)subpart;}
+
+ virtual void unLockReadOnlyVertexBase(int subpart) const {(void)subpart;}
+
+ /// getNumSubParts returns the number of seperate subparts
+ /// each subpart has a continuous array of vertices and indices
+ virtual int getNumSubParts() const {
+ return (int)m_indexedMeshes.size();
+ }
+
+ IndexedMeshArray& getIndexedMeshArray()
+ {
+ return m_indexedMeshes;
+ }
+
+ const IndexedMeshArray& getIndexedMeshArray() const
+ {
+ return m_indexedMeshes;
+ }
+
+ virtual void preallocateVertices(int numverts){(void) numverts;}
+ virtual void preallocateIndices(int numindices){(void) numindices;}
+
+ virtual bool hasPremadeAabb() const;
+ virtual void setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax ) const;
+ virtual void getPremadeAabb(btVector3* aabbMin, btVector3* aabbMax ) const;
+
+}
+;
+
+#endif //BT_TRIANGLE_INDEX_VERTEX_ARRAY_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,84 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///This file was created by Alex Silverman
+
+#ifndef BT_MULTIMATERIAL_TRIANGLE_INDEX_VERTEX_ARRAY_H
+#define BT_MULTIMATERIAL_TRIANGLE_INDEX_VERTEX_ARRAY_H
+
+#include "btTriangleIndexVertexArray.h"
+
+
+ATTRIBUTE_ALIGNED16( struct) btMaterialProperties
+{
+ ///m_materialBase ==========> 2 btScalar values make up one material, friction then restitution
+ int m_numMaterials;
+ const unsigned char * m_materialBase;
+ int m_materialStride;
+ PHY_ScalarType m_materialType;
+ ///m_numTriangles <=========== This exists in the btIndexedMesh object for the same subpart, but since we're
+ /// padding the structure, it can be reproduced at no real cost
+ ///m_triangleMaterials =====> 1 integer value makes up one entry
+ /// eg: m_triangleMaterials[1] = 5; // This will set triangle 2 to use material 5
+ int m_numTriangles;
+ const unsigned char * m_triangleMaterialsBase;
+ int m_triangleMaterialStride;
+ ///m_triangleType <========== Automatically set in addMaterialProperties
+ PHY_ScalarType m_triangleType;
+};
+
+typedef btAlignedObjectArray<btMaterialProperties> MaterialArray;
+
+///Teh btTriangleIndexVertexMaterialArray is built on TriangleIndexVertexArray
+///The addition of a material array allows for the utilization of the partID and
+///triangleIndex that are returned in the ContactAddedCallback. As with
+///TriangleIndexVertexArray, no duplicate is made of the material data, so it
+///is the users responsibility to maintain the array during the lifetime of the
+///TriangleIndexVertexMaterialArray.
+ATTRIBUTE_ALIGNED16(class) btTriangleIndexVertexMaterialArray : public btTriangleIndexVertexArray
+{
+protected:
+ MaterialArray m_materials;
+
+public:
+ BT_DECLARE_ALIGNED_ALLOCATOR();
+
+ btTriangleIndexVertexMaterialArray()
+ {
+ }
+
+ btTriangleIndexVertexMaterialArray(int numTriangles,int* triangleIndexBase,int triangleIndexStride,
+ int numVertices,btScalar* vertexBase,int vertexStride,
+ int numMaterials, unsigned char* materialBase, int materialStride,
+ int* triangleMaterialsBase, int materialIndexStride);
+
+ virtual ~btTriangleIndexVertexMaterialArray() {}
+
+ void addMaterialProperties(const btMaterialProperties& mat, PHY_ScalarType triangleType = PHY_INTEGER)
+ {
+ m_materials.push_back(mat);
+ m_materials[m_materials.size()-1].m_triangleType = triangleType;
+ }
+
+ virtual void getLockedMaterialBase(unsigned char **materialBase, int& numMaterials, PHY_ScalarType& materialType, int& materialStride,
+ unsigned char ** triangleMaterialBase, int& numTriangles, int& triangleMaterialStride, PHY_ScalarType& triangleType ,int subpart = 0);
+
+ virtual void getLockedReadOnlyMaterialBase(const unsigned char **materialBase, int& numMaterials, PHY_ScalarType& materialType, int& materialStride,
+ const unsigned char ** triangleMaterialBase, int& numTriangles, int& triangleMaterialStride, PHY_ScalarType& triangleType, int subpart = 0);
+
+}
+;
+
+#endif //BT_MULTIMATERIAL_TRIANGLE_INDEX_VERTEX_ARRAY_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btTriangleMesh.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btTriangleMesh.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btTriangleMesh.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btTriangleMesh.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,69 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef TRIANGLE_MESH_H
+#define TRIANGLE_MESH_H
+
+#include "btTriangleIndexVertexArray.h"
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btAlignedObjectArray.h"
+
+///The btTriangleMesh class is a convenience class derived from btTriangleIndexVertexArray, that provides storage for a concave triangle mesh. It can be used as data for the btBvhTriangleMeshShape.
+///It allows either 32bit or 16bit indices, and 4 (x-y-z-w) or 3 (x-y-z) component vertices.
+///If you want to share triangle/index data between graphics mesh and collision mesh (btBvhTriangleMeshShape), you can directly use btTriangleIndexVertexArray or derive your own class from btStridingMeshInterface.
+///Performance of btTriangleMesh and btTriangleIndexVertexArray used in a btBvhTriangleMeshShape is the same.
+class btTriangleMesh : public btTriangleIndexVertexArray
+{
+ btAlignedObjectArray<btVector3> m_4componentVertices;
+ btAlignedObjectArray<float> m_3componentVertices;
+
+ btAlignedObjectArray<unsigned int> m_32bitIndices;
+ btAlignedObjectArray<unsigned short int> m_16bitIndices;
+ bool m_use32bitIndices;
+ bool m_use4componentVertices;
+
+
+ public:
+ btScalar m_weldingThreshold;
+
+ btTriangleMesh (bool use32bitIndices=true,bool use4componentVertices=true);
+
+ bool getUse32bitIndices() const
+ {
+ return m_use32bitIndices;
+ }
+
+ bool getUse4componentVertices() const
+ {
+ return m_use4componentVertices;
+ }
+ ///By default addTriangle won't search for duplicate vertices, because the search is very slow for large triangle meshes.
+ ///In general it is better to directly use btTriangleIndexVertexArray instead.
+ void addTriangle(const btVector3& vertex0,const btVector3& vertex1,const btVector3& vertex2, bool removeDuplicateVertices=false);
+
+ int getNumTriangles() const;
+
+ virtual void preallocateVertices(int numverts){(void) numverts;}
+ virtual void preallocateIndices(int numindices){(void) numindices;}
+
+ ///findOrAddVertex is an internal method, use addTriangle instead
+ int findOrAddVertex(const btVector3& vertex, bool removeDuplicateVertices);
+ ///addIndex is an internal method, use addTriangle instead
+ void addIndex(int index);
+
+};
+
+#endif //TRIANGLE_MESH_H
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btTriangleMeshShape.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btTriangleMeshShape.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btTriangleMeshShape.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btTriangleMeshShape.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,85 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef TRIANGLE_MESH_SHAPE_H
+#define TRIANGLE_MESH_SHAPE_H
+
+#include "btConcaveShape.h"
+#include "btStridingMeshInterface.h"
+
+
+///The btTriangleMeshShape is an internal concave triangle mesh interface. Don't use this class directly, use btBvhTriangleMeshShape instead.
+class btTriangleMeshShape : public btConcaveShape
+{
+protected:
+ btVector3 m_localAabbMin;
+ btVector3 m_localAabbMax;
+ btStridingMeshInterface* m_meshInterface;
+
+ ///btTriangleMeshShape constructor has been disabled/protected, so that users will not mistakenly use this class.
+ ///Don't use btTriangleMeshShape but use btBvhTriangleMeshShape instead!
+ btTriangleMeshShape(btStridingMeshInterface* meshInterface);
+
+public:
+
+ virtual ~btTriangleMeshShape();
+
+ virtual btVector3 localGetSupportingVertex(const btVector3& vec) const;
+
+ virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const
+ {
+ btAssert(0);
+ return localGetSupportingVertex(vec);
+ }
+
+ void recalcLocalAabb();
+
+ virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
+
+ virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const;
+
+ virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+
+ virtual void setLocalScaling(const btVector3& scaling);
+ virtual const btVector3& getLocalScaling() const;
+
+ btStridingMeshInterface* getMeshInterface()
+ {
+ return m_meshInterface;
+ }
+
+ const btStridingMeshInterface* getMeshInterface() const
+ {
+ return m_meshInterface;
+ }
+
+ const btVector3& getLocalAabbMin() const
+ {
+ return m_localAabbMin;
+ }
+ const btVector3& getLocalAabbMax() const
+ {
+ return m_localAabbMax;
+ }
+
+
+
+ //debugging
+ virtual const char* getName()const {return "TRIANGLEMESH";}
+
+
+};
+
+#endif //TRIANGLE_MESH_SHAPE_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btTriangleShape.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btTriangleShape.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btTriangleShape.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btTriangleShape.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,182 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef OBB_TRIANGLE_MINKOWSKI_H
+#define OBB_TRIANGLE_MINKOWSKI_H
+
+#include "btConvexShape.h"
+#include "btBoxShape.h"
+
+ATTRIBUTE_ALIGNED16(class) btTriangleShape : public btPolyhedralConvexShape
+{
+
+
+public:
+
+ btVector3 m_vertices1[3];
+
+ virtual int getNumVertices() const
+ {
+ return 3;
+ }
+
+ btVector3& getVertexPtr(int index)
+ {
+ return m_vertices1[index];
+ }
+
+ const btVector3& getVertexPtr(int index) const
+ {
+ return m_vertices1[index];
+ }
+ virtual void getVertex(int index,btVector3& vert) const
+ {
+ vert = m_vertices1[index];
+ }
+
+ virtual int getNumEdges() const
+ {
+ return 3;
+ }
+
+ virtual void getEdge(int i,btVector3& pa,btVector3& pb) const
+ {
+ getVertex(i,pa);
+ getVertex((i+1)%3,pb);
+ }
+
+
+ virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax)const
+ {
+// btAssert(0);
+ getAabbSlow(t,aabbMin,aabbMax);
+ }
+
+ btVector3 localGetSupportingVertexWithoutMargin(const btVector3& dir)const
+ {
+ btVector3 dots(dir.dot(m_vertices1[0]), dir.dot(m_vertices1[1]), dir.dot(m_vertices1[2]));
+ return m_vertices1[dots.maxAxis()];
+
+ }
+
+ virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
+ {
+ for (int i=0;i<numVectors;i++)
+ {
+ const btVector3& dir = vectors[i];
+ btVector3 dots(dir.dot(m_vertices1[0]), dir.dot(m_vertices1[1]), dir.dot(m_vertices1[2]));
+ supportVerticesOut[i] = m_vertices1[dots.maxAxis()];
+ }
+
+ }
+
+ btTriangleShape() : btPolyhedralConvexShape ()
+ {
+ m_shapeType = TRIANGLE_SHAPE_PROXYTYPE;
+ }
+
+ btTriangleShape(const btVector3& p0,const btVector3& p1,const btVector3& p2) : btPolyhedralConvexShape ()
+ {
+ m_shapeType = TRIANGLE_SHAPE_PROXYTYPE;
+ m_vertices1[0] = p0;
+ m_vertices1[1] = p1;
+ m_vertices1[2] = p2;
+ }
+
+
+ virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i) const
+ {
+ getPlaneEquation(i,planeNormal,planeSupport);
+ }
+
+ virtual int getNumPlanes() const
+ {
+ return 1;
+ }
+
+ void calcNormal(btVector3& normal) const
+ {
+ normal = (m_vertices1[1]-m_vertices1[0]).cross(m_vertices1[2]-m_vertices1[0]);
+ normal.normalize();
+ }
+
+ virtual void getPlaneEquation(int i, btVector3& planeNormal,btVector3& planeSupport) const
+ {
+ (void)i;
+ calcNormal(planeNormal);
+ planeSupport = m_vertices1[0];
+ }
+
+ virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const
+ {
+ (void)mass;
+ btAssert(0);
+ inertia.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+ }
+
+ virtual bool isInside(const btVector3& pt,btScalar tolerance) const
+ {
+ btVector3 normal;
+ calcNormal(normal);
+ //distance to plane
+ btScalar dist = pt.dot(normal);
+ btScalar planeconst = m_vertices1[0].dot(normal);
+ dist -= planeconst;
+ if (dist >= -tolerance && dist <= tolerance)
+ {
+ //inside check on edge-planes
+ int i;
+ for (i=0;i<3;i++)
+ {
+ btVector3 pa,pb;
+ getEdge(i,pa,pb);
+ btVector3 edge = pb-pa;
+ btVector3 edgeNormal = edge.cross(normal);
+ edgeNormal.normalize();
+ btScalar dist = pt.dot( edgeNormal);
+ btScalar edgeConst = pa.dot(edgeNormal);
+ dist -= edgeConst;
+ if (dist < -tolerance)
+ return false;
+ }
+
+ return true;
+ }
+
+ return false;
+ }
+ //debugging
+ virtual const char* getName()const
+ {
+ return "Triangle";
+ }
+
+ virtual int getNumPreferredPenetrationDirections() const
+ {
+ return 2;
+ }
+
+ virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const
+ {
+ calcNormal(penetrationVector);
+ if (index)
+ penetrationVector *= btScalar(-1.);
+ }
+
+
+};
+
+#endif //OBB_TRIANGLE_MINKOWSKI_H
+
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btUniformScalingShape.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btUniformScalingShape.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btUniformScalingShape.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/CollisionShapes/btUniformScalingShape.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,87 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_UNIFORM_SCALING_SHAPE_H
+#define BT_UNIFORM_SCALING_SHAPE_H
+
+#include "btConvexShape.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
+
+///The btUniformScalingShape allows to re-use uniform scaled instances of btConvexShape in a memory efficient way.
+///Istead of using btUniformScalingShape, it is better to use the non-uniform setLocalScaling method on convex shapes that implement it.
+class btUniformScalingShape : public btConvexShape
+{
+ btConvexShape* m_childConvexShape;
+
+ btScalar m_uniformScalingFactor;
+
+ public:
+
+ btUniformScalingShape( btConvexShape* convexChildShape, btScalar uniformScalingFactor);
+
+ virtual ~btUniformScalingShape();
+
+ virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
+
+ virtual btVector3 localGetSupportingVertex(const btVector3& vec)const;
+
+ virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
+
+ virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+
+ btScalar getUniformScalingFactor() const
+ {
+ return m_uniformScalingFactor;
+ }
+
+ btConvexShape* getChildShape()
+ {
+ return m_childConvexShape;
+ }
+
+ const btConvexShape* getChildShape() const
+ {
+ return m_childConvexShape;
+ }
+
+ virtual const char* getName()const
+ {
+ return "UniformScalingShape";
+ }
+
+
+
+ ///////////////////////////
+
+
+ ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
+ void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
+
+ virtual void getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
+
+ virtual void setLocalScaling(const btVector3& scaling) ;
+ virtual const btVector3& getLocalScaling() const ;
+
+ virtual void setMargin(btScalar margin);
+ virtual btScalar getMargin() const;
+
+ virtual int getNumPreferredPenetrationDirections() const;
+
+ virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const;
+
+
+};
+
+#endif //BT_UNIFORM_SCALING_SHAPE_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btBoxCollision.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btBoxCollision.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btBoxCollision.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btBoxCollision.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,647 @@
+#ifndef BT_BOX_COLLISION_H_INCLUDED
+#define BT_BOX_COLLISION_H_INCLUDED
+
+/*! \file gim_box_collision.h
+\author Francisco Len Nßjera
+*/
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman at yahoo.com
+
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "LinearMath/btTransform.h"
+
+
+///Swap numbers
+#define BT_SWAP_NUMBERS(a,b){ \
+ a = a+b; \
+ b = a-b; \
+ a = a-b; \
+}\
+
+
+#define BT_MAX(a,b) (a<b?b:a)
+#define BT_MIN(a,b) (a>b?b:a)
+
+#define BT_GREATER(x, y) btFabs(x) > (y)
+
+#define BT_MAX3(a,b,c) BT_MAX(a,BT_MAX(b,c))
+#define BT_MIN3(a,b,c) BT_MIN(a,BT_MIN(b,c))
+
+
+
+
+
+
+enum eBT_PLANE_INTERSECTION_TYPE
+{
+ BT_CONST_BACK_PLANE = 0,
+ BT_CONST_COLLIDE_PLANE,
+ BT_CONST_FRONT_PLANE
+};
+
+//SIMD_FORCE_INLINE bool test_cross_edge_box(
+// const btVector3 & edge,
+// const btVector3 & absolute_edge,
+// const btVector3 & pointa,
+// const btVector3 & pointb, const btVector3 & extend,
+// int dir_index0,
+// int dir_index1
+// int component_index0,
+// int component_index1)
+//{
+// // dir coords are -z and y
+//
+// const btScalar dir0 = -edge[dir_index0];
+// const btScalar dir1 = edge[dir_index1];
+// btScalar pmin = pointa[component_index0]*dir0 + pointa[component_index1]*dir1;
+// btScalar pmax = pointb[component_index0]*dir0 + pointb[component_index1]*dir1;
+// //find minmax
+// if(pmin>pmax)
+// {
+// BT_SWAP_NUMBERS(pmin,pmax);
+// }
+// //find extends
+// const btScalar rad = extend[component_index0] * absolute_edge[dir_index0] +
+// extend[component_index1] * absolute_edge[dir_index1];
+//
+// if(pmin>rad || -rad>pmax) return false;
+// return true;
+//}
+//
+//SIMD_FORCE_INLINE bool test_cross_edge_box_X_axis(
+// const btVector3 & edge,
+// const btVector3 & absolute_edge,
+// const btVector3 & pointa,
+// const btVector3 & pointb, btVector3 & extend)
+//{
+//
+// return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,2,1,1,2);
+//}
+//
+//
+//SIMD_FORCE_INLINE bool test_cross_edge_box_Y_axis(
+// const btVector3 & edge,
+// const btVector3 & absolute_edge,
+// const btVector3 & pointa,
+// const btVector3 & pointb, btVector3 & extend)
+//{
+//
+// return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,0,2,2,0);
+//}
+//
+//SIMD_FORCE_INLINE bool test_cross_edge_box_Z_axis(
+// const btVector3 & edge,
+// const btVector3 & absolute_edge,
+// const btVector3 & pointa,
+// const btVector3 & pointb, btVector3 & extend)
+//{
+//
+// return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,1,0,0,1);
+//}
+
+
+#define TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,i_dir_0,i_dir_1,i_comp_0,i_comp_1)\
+{\
+ const btScalar dir0 = -edge[i_dir_0];\
+ const btScalar dir1 = edge[i_dir_1];\
+ btScalar pmin = pointa[i_comp_0]*dir0 + pointa[i_comp_1]*dir1;\
+ btScalar pmax = pointb[i_comp_0]*dir0 + pointb[i_comp_1]*dir1;\
+ if(pmin>pmax)\
+ {\
+ BT_SWAP_NUMBERS(pmin,pmax); \
+ }\
+ const btScalar abs_dir0 = absolute_edge[i_dir_0];\
+ const btScalar abs_dir1 = absolute_edge[i_dir_1];\
+ const btScalar rad = _extend[i_comp_0] * abs_dir0 + _extend[i_comp_1] * abs_dir1;\
+ if(pmin>rad || -rad>pmax) return false;\
+}\
+
+
+#define TEST_CROSS_EDGE_BOX_X_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\
+{\
+ TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,2,1,1,2);\
+}\
+
+#define TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\
+{\
+ TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,0,2,2,0);\
+}\
+
+#define TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\
+{\
+ TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,1,0,0,1);\
+}\
+
+
+//! Returns the dot product between a vec3f and the col of a matrix
+SIMD_FORCE_INLINE btScalar bt_mat3_dot_col(
+const btMatrix3x3 & mat, const btVector3 & vec3, int colindex)
+{
+ return vec3[0]*mat[0][colindex] + vec3[1]*mat[1][colindex] + vec3[2]*mat[2][colindex];
+}
+
+
+//! Class for transforming a model1 to the space of model0
+ATTRIBUTE_ALIGNED16 (class) BT_BOX_BOX_TRANSFORM_CACHE
+{
+public:
+ btVector3 m_T1to0;//!< Transforms translation of model1 to model 0
+ btMatrix3x3 m_R1to0;//!< Transforms Rotation of model1 to model 0, equal to R0' * R1
+ btMatrix3x3 m_AR;//!< Absolute value of m_R1to0
+
+ SIMD_FORCE_INLINE void calc_absolute_matrix()
+ {
+// static const btVector3 vepsi(1e-6f,1e-6f,1e-6f);
+// m_AR[0] = vepsi + m_R1to0[0].absolute();
+// m_AR[1] = vepsi + m_R1to0[1].absolute();
+// m_AR[2] = vepsi + m_R1to0[2].absolute();
+
+ int i,j;
+
+ for(i=0;i<3;i++)
+ {
+ for(j=0;j<3;j++ )
+ {
+ m_AR[i][j] = 1e-6f + btFabs(m_R1to0[i][j]);
+ }
+ }
+
+ }
+
+ BT_BOX_BOX_TRANSFORM_CACHE()
+ {
+ }
+
+
+
+ //! Calc the transformation relative 1 to 0. Inverts matrics by transposing
+ SIMD_FORCE_INLINE void calc_from_homogenic(const btTransform & trans0,const btTransform & trans1)
+ {
+
+ btTransform temp_trans = trans0.inverse();
+ temp_trans = temp_trans * trans1;
+
+ m_T1to0 = temp_trans.getOrigin();
+ m_R1to0 = temp_trans.getBasis();
+
+
+ calc_absolute_matrix();
+ }
+
+ //! Calcs the full invertion of the matrices. Useful for scaling matrices
+ SIMD_FORCE_INLINE void calc_from_full_invert(const btTransform & trans0,const btTransform & trans1)
+ {
+ m_R1to0 = trans0.getBasis().inverse();
+ m_T1to0 = m_R1to0 * (-trans0.getOrigin());
+
+ m_T1to0 += m_R1to0*trans1.getOrigin();
+ m_R1to0 *= trans1.getBasis();
+
+ calc_absolute_matrix();
+ }
+
+ SIMD_FORCE_INLINE btVector3 transform(const btVector3 & point) const
+ {
+ return btVector3(m_R1to0[0].dot(point) + m_T1to0.x(),
+ m_R1to0[1].dot(point) + m_T1to0.y(),
+ m_R1to0[2].dot(point) + m_T1to0.z());
+ }
+};
+
+
+#define BOX_PLANE_EPSILON 0.000001f
+
+//! Axis aligned box
+ATTRIBUTE_ALIGNED16 (class) btAABB
+{
+public:
+ btVector3 m_min;
+ btVector3 m_max;
+
+ btAABB()
+ {}
+
+
+ btAABB(const btVector3 & V1,
+ const btVector3 & V2,
+ const btVector3 & V3)
+ {
+ m_min[0] = BT_MIN3(V1[0],V2[0],V3[0]);
+ m_min[1] = BT_MIN3(V1[1],V2[1],V3[1]);
+ m_min[2] = BT_MIN3(V1[2],V2[2],V3[2]);
+
+ m_max[0] = BT_MAX3(V1[0],V2[0],V3[0]);
+ m_max[1] = BT_MAX3(V1[1],V2[1],V3[1]);
+ m_max[2] = BT_MAX3(V1[2],V2[2],V3[2]);
+ }
+
+ btAABB(const btVector3 & V1,
+ const btVector3 & V2,
+ const btVector3 & V3,
+ btScalar margin)
+ {
+ m_min[0] = BT_MIN3(V1[0],V2[0],V3[0]);
+ m_min[1] = BT_MIN3(V1[1],V2[1],V3[1]);
+ m_min[2] = BT_MIN3(V1[2],V2[2],V3[2]);
+
+ m_max[0] = BT_MAX3(V1[0],V2[0],V3[0]);
+ m_max[1] = BT_MAX3(V1[1],V2[1],V3[1]);
+ m_max[2] = BT_MAX3(V1[2],V2[2],V3[2]);
+
+ m_min[0] -= margin;
+ m_min[1] -= margin;
+ m_min[2] -= margin;
+ m_max[0] += margin;
+ m_max[1] += margin;
+ m_max[2] += margin;
+ }
+
+ btAABB(const btAABB &other):
+ m_min(other.m_min),m_max(other.m_max)
+ {
+ }
+
+ btAABB(const btAABB &other,btScalar margin ):
+ m_min(other.m_min),m_max(other.m_max)
+ {
+ m_min[0] -= margin;
+ m_min[1] -= margin;
+ m_min[2] -= margin;
+ m_max[0] += margin;
+ m_max[1] += margin;
+ m_max[2] += margin;
+ }
+
+ SIMD_FORCE_INLINE void invalidate()
+ {
+ m_min[0] = SIMD_INFINITY;
+ m_min[1] = SIMD_INFINITY;
+ m_min[2] = SIMD_INFINITY;
+ m_max[0] = -SIMD_INFINITY;
+ m_max[1] = -SIMD_INFINITY;
+ m_max[2] = -SIMD_INFINITY;
+ }
+
+ SIMD_FORCE_INLINE void increment_margin(btScalar margin)
+ {
+ m_min[0] -= margin;
+ m_min[1] -= margin;
+ m_min[2] -= margin;
+ m_max[0] += margin;
+ m_max[1] += margin;
+ m_max[2] += margin;
+ }
+
+ SIMD_FORCE_INLINE void copy_with_margin(const btAABB &other, btScalar margin)
+ {
+ m_min[0] = other.m_min[0] - margin;
+ m_min[1] = other.m_min[1] - margin;
+ m_min[2] = other.m_min[2] - margin;
+
+ m_max[0] = other.m_max[0] + margin;
+ m_max[1] = other.m_max[1] + margin;
+ m_max[2] = other.m_max[2] + margin;
+ }
+
+ template<typename CLASS_POINT>
+ SIMD_FORCE_INLINE void calc_from_triangle(
+ const CLASS_POINT & V1,
+ const CLASS_POINT & V2,
+ const CLASS_POINT & V3)
+ {
+ m_min[0] = BT_MIN3(V1[0],V2[0],V3[0]);
+ m_min[1] = BT_MIN3(V1[1],V2[1],V3[1]);
+ m_min[2] = BT_MIN3(V1[2],V2[2],V3[2]);
+
+ m_max[0] = BT_MAX3(V1[0],V2[0],V3[0]);
+ m_max[1] = BT_MAX3(V1[1],V2[1],V3[1]);
+ m_max[2] = BT_MAX3(V1[2],V2[2],V3[2]);
+ }
+
+ template<typename CLASS_POINT>
+ SIMD_FORCE_INLINE void calc_from_triangle_margin(
+ const CLASS_POINT & V1,
+ const CLASS_POINT & V2,
+ const CLASS_POINT & V3, btScalar margin)
+ {
+ m_min[0] = BT_MIN3(V1[0],V2[0],V3[0]);
+ m_min[1] = BT_MIN3(V1[1],V2[1],V3[1]);
+ m_min[2] = BT_MIN3(V1[2],V2[2],V3[2]);
+
+ m_max[0] = BT_MAX3(V1[0],V2[0],V3[0]);
+ m_max[1] = BT_MAX3(V1[1],V2[1],V3[1]);
+ m_max[2] = BT_MAX3(V1[2],V2[2],V3[2]);
+
+ m_min[0] -= margin;
+ m_min[1] -= margin;
+ m_min[2] -= margin;
+ m_max[0] += margin;
+ m_max[1] += margin;
+ m_max[2] += margin;
+ }
+
+ //! Apply a transform to an AABB
+ SIMD_FORCE_INLINE void appy_transform(const btTransform & trans)
+ {
+ btVector3 center = (m_max+m_min)*0.5f;
+ btVector3 extends = m_max - center;
+ // Compute new center
+ center = trans(center);
+
+ btVector3 textends(extends.dot(trans.getBasis().getRow(0).absolute()),
+ extends.dot(trans.getBasis().getRow(1).absolute()),
+ extends.dot(trans.getBasis().getRow(2).absolute()));
+
+ m_min = center - textends;
+ m_max = center + textends;
+ }
+
+
+ //! Apply a transform to an AABB
+ SIMD_FORCE_INLINE void appy_transform_trans_cache(const BT_BOX_BOX_TRANSFORM_CACHE & trans)
+ {
+ btVector3 center = (m_max+m_min)*0.5f;
+ btVector3 extends = m_max - center;
+ // Compute new center
+ center = trans.transform(center);
+
+ btVector3 textends(extends.dot(trans.m_R1to0.getRow(0).absolute()),
+ extends.dot(trans.m_R1to0.getRow(1).absolute()),
+ extends.dot(trans.m_R1to0.getRow(2).absolute()));
+
+ m_min = center - textends;
+ m_max = center + textends;
+ }
+
+ //! Merges a Box
+ SIMD_FORCE_INLINE void merge(const btAABB & box)
+ {
+ m_min[0] = BT_MIN(m_min[0],box.m_min[0]);
+ m_min[1] = BT_MIN(m_min[1],box.m_min[1]);
+ m_min[2] = BT_MIN(m_min[2],box.m_min[2]);
+
+ m_max[0] = BT_MAX(m_max[0],box.m_max[0]);
+ m_max[1] = BT_MAX(m_max[1],box.m_max[1]);
+ m_max[2] = BT_MAX(m_max[2],box.m_max[2]);
+ }
+
+ //! Merges a point
+ template<typename CLASS_POINT>
+ SIMD_FORCE_INLINE void merge_point(const CLASS_POINT & point)
+ {
+ m_min[0] = BT_MIN(m_min[0],point[0]);
+ m_min[1] = BT_MIN(m_min[1],point[1]);
+ m_min[2] = BT_MIN(m_min[2],point[2]);
+
+ m_max[0] = BT_MAX(m_max[0],point[0]);
+ m_max[1] = BT_MAX(m_max[1],point[1]);
+ m_max[2] = BT_MAX(m_max[2],point[2]);
+ }
+
+ //! Gets the extend and center
+ SIMD_FORCE_INLINE void get_center_extend(btVector3 & center,btVector3 & extend) const
+ {
+ center = (m_max+m_min)*0.5f;
+ extend = m_max - center;
+ }
+
+ //! Finds the intersecting box between this box and the other.
+ SIMD_FORCE_INLINE void find_intersection(const btAABB & other, btAABB & intersection) const
+ {
+ intersection.m_min[0] = BT_MAX(other.m_min[0],m_min[0]);
+ intersection.m_min[1] = BT_MAX(other.m_min[1],m_min[1]);
+ intersection.m_min[2] = BT_MAX(other.m_min[2],m_min[2]);
+
+ intersection.m_max[0] = BT_MIN(other.m_max[0],m_max[0]);
+ intersection.m_max[1] = BT_MIN(other.m_max[1],m_max[1]);
+ intersection.m_max[2] = BT_MIN(other.m_max[2],m_max[2]);
+ }
+
+
+ SIMD_FORCE_INLINE bool has_collision(const btAABB & other) const
+ {
+ if(m_min[0] > other.m_max[0] ||
+ m_max[0] < other.m_min[0] ||
+ m_min[1] > other.m_max[1] ||
+ m_max[1] < other.m_min[1] ||
+ m_min[2] > other.m_max[2] ||
+ m_max[2] < other.m_min[2])
+ {
+ return false;
+ }
+ return true;
+ }
+
+ /*! \brief Finds the Ray intersection parameter.
+ \param aabb Aligned box
+ \param vorigin A vec3f with the origin of the ray
+ \param vdir A vec3f with the direction of the ray
+ */
+ SIMD_FORCE_INLINE bool collide_ray(const btVector3 & vorigin,const btVector3 & vdir) const
+ {
+ btVector3 extents,center;
+ this->get_center_extend(center,extents);;
+
+ btScalar Dx = vorigin[0] - center[0];
+ if(BT_GREATER(Dx, extents[0]) && Dx*vdir[0]>=0.0f) return false;
+ btScalar Dy = vorigin[1] - center[1];
+ if(BT_GREATER(Dy, extents[1]) && Dy*vdir[1]>=0.0f) return false;
+ btScalar Dz = vorigin[2] - center[2];
+ if(BT_GREATER(Dz, extents[2]) && Dz*vdir[2]>=0.0f) return false;
+
+
+ btScalar f = vdir[1] * Dz - vdir[2] * Dy;
+ if(btFabs(f) > extents[1]*btFabs(vdir[2]) + extents[2]*btFabs(vdir[1])) return false;
+ f = vdir[2] * Dx - vdir[0] * Dz;
+ if(btFabs(f) > extents[0]*btFabs(vdir[2]) + extents[2]*btFabs(vdir[0]))return false;
+ f = vdir[0] * Dy - vdir[1] * Dx;
+ if(btFabs(f) > extents[0]*btFabs(vdir[1]) + extents[1]*btFabs(vdir[0]))return false;
+ return true;
+ }
+
+
+ SIMD_FORCE_INLINE void projection_interval(const btVector3 & direction, btScalar &vmin, btScalar &vmax) const
+ {
+ btVector3 center = (m_max+m_min)*0.5f;
+ btVector3 extend = m_max-center;
+
+ btScalar _fOrigin = direction.dot(center);
+ btScalar _fMaximumExtent = extend.dot(direction.absolute());
+ vmin = _fOrigin - _fMaximumExtent;
+ vmax = _fOrigin + _fMaximumExtent;
+ }
+
+ SIMD_FORCE_INLINE eBT_PLANE_INTERSECTION_TYPE plane_classify(const btVector4 &plane) const
+ {
+ btScalar _fmin,_fmax;
+ this->projection_interval(plane,_fmin,_fmax);
+
+ if(plane[3] > _fmax + BOX_PLANE_EPSILON)
+ {
+ return BT_CONST_BACK_PLANE; // 0
+ }
+
+ if(plane[3]+BOX_PLANE_EPSILON >=_fmin)
+ {
+ return BT_CONST_COLLIDE_PLANE; //1
+ }
+ return BT_CONST_FRONT_PLANE;//2
+ }
+
+ SIMD_FORCE_INLINE bool overlapping_trans_conservative(const btAABB & box, btTransform & trans1_to_0) const
+ {
+ btAABB tbox = box;
+ tbox.appy_transform(trans1_to_0);
+ return has_collision(tbox);
+ }
+
+ SIMD_FORCE_INLINE bool overlapping_trans_conservative2(const btAABB & box,
+ const BT_BOX_BOX_TRANSFORM_CACHE & trans1_to_0) const
+ {
+ btAABB tbox = box;
+ tbox.appy_transform_trans_cache(trans1_to_0);
+ return has_collision(tbox);
+ }
+
+ //! transcache is the transformation cache from box to this AABB
+ SIMD_FORCE_INLINE bool overlapping_trans_cache(
+ const btAABB & box,const BT_BOX_BOX_TRANSFORM_CACHE & transcache, bool fulltest) const
+ {
+
+ //Taken from OPCODE
+ btVector3 ea,eb;//extends
+ btVector3 ca,cb;//extends
+ get_center_extend(ca,ea);
+ box.get_center_extend(cb,eb);
+
+
+ btVector3 T;
+ btScalar t,t2;
+ int i;
+
+ // Class I : A's basis vectors
+ for(i=0;i<3;i++)
+ {
+ T[i] = transcache.m_R1to0[i].dot(cb) + transcache.m_T1to0[i] - ca[i];
+ t = transcache.m_AR[i].dot(eb) + ea[i];
+ if(BT_GREATER(T[i], t)) return false;
+ }
+ // Class II : B's basis vectors
+ for(i=0;i<3;i++)
+ {
+ t = bt_mat3_dot_col(transcache.m_R1to0,T,i);
+ t2 = bt_mat3_dot_col(transcache.m_AR,ea,i) + eb[i];
+ if(BT_GREATER(t,t2)) return false;
+ }
+ // Class III : 9 cross products
+ if(fulltest)
+ {
+ int j,m,n,o,p,q,r;
+ for(i=0;i<3;i++)
+ {
+ m = (i+1)%3;
+ n = (i+2)%3;
+ o = i==0?1:0;
+ p = i==2?1:2;
+ for(j=0;j<3;j++)
+ {
+ q = j==2?1:2;
+ r = j==0?1:0;
+ t = T[n]*transcache.m_R1to0[m][j] - T[m]*transcache.m_R1to0[n][j];
+ t2 = ea[o]*transcache.m_AR[p][j] + ea[p]*transcache.m_AR[o][j] +
+ eb[r]*transcache.m_AR[i][q] + eb[q]*transcache.m_AR[i][r];
+ if(BT_GREATER(t,t2)) return false;
+ }
+ }
+ }
+ return true;
+ }
+
+ //! Simple test for planes.
+ SIMD_FORCE_INLINE bool collide_plane(
+ const btVector4 & plane) const
+ {
+ eBT_PLANE_INTERSECTION_TYPE classify = plane_classify(plane);
+ return (classify == BT_CONST_COLLIDE_PLANE);
+ }
+
+ //! test for a triangle, with edges
+ SIMD_FORCE_INLINE bool collide_triangle_exact(
+ const btVector3 & p1,
+ const btVector3 & p2,
+ const btVector3 & p3,
+ const btVector4 & triangle_plane) const
+ {
+ if(!collide_plane(triangle_plane)) return false;
+
+ btVector3 center,extends;
+ this->get_center_extend(center,extends);
+
+ const btVector3 v1(p1 - center);
+ const btVector3 v2(p2 - center);
+ const btVector3 v3(p3 - center);
+
+ //First axis
+ btVector3 diff(v2 - v1);
+ btVector3 abs_diff = diff.absolute();
+ //Test With X axis
+ TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v1,v3,extends);
+ //Test With Y axis
+ TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v1,v3,extends);
+ //Test With Z axis
+ TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v1,v3,extends);
+
+
+ diff = v3 - v2;
+ abs_diff = diff.absolute();
+ //Test With X axis
+ TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v2,v1,extends);
+ //Test With Y axis
+ TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v2,v1,extends);
+ //Test With Z axis
+ TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v2,v1,extends);
+
+ diff = v1 - v3;
+ abs_diff = diff.absolute();
+ //Test With X axis
+ TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v3,v2,extends);
+ //Test With Y axis
+ TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v3,v2,extends);
+ //Test With Z axis
+ TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v3,v2,extends);
+
+ return true;
+ }
+};
+
+
+//! Compairison of transformation objects
+SIMD_FORCE_INLINE bool btCompareTransformsEqual(const btTransform & t1,const btTransform & t2)
+{
+ if(!(t1.getOrigin() == t2.getOrigin()) ) return false;
+
+ if(!(t1.getBasis().getRow(0) == t2.getBasis().getRow(0)) ) return false;
+ if(!(t1.getBasis().getRow(1) == t2.getBasis().getRow(1)) ) return false;
+ if(!(t1.getBasis().getRow(2) == t2.getBasis().getRow(2)) ) return false;
+ return true;
+}
+
+
+
+#endif // GIM_BOX_COLLISION_H_INCLUDED
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btClipPolygon.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btClipPolygon.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btClipPolygon.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btClipPolygon.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,182 @@
+#ifndef BT_CLIP_POLYGON_H_INCLUDED
+#define BT_CLIP_POLYGON_H_INCLUDED
+
+/*! \file btClipPolygon.h
+\author Francisco Len Nßjera
+*/
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman at yahoo.com
+
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btGeometryUtil.h"
+
+
+SIMD_FORCE_INLINE btScalar bt_distance_point_plane(const btVector4 & plane,const btVector3 &point)
+{
+ return point.dot(plane) - plane[3];
+}
+
+/*! Vector blending
+Takes two vectors a, b, blends them together*/
+SIMD_FORCE_INLINE void bt_vec_blend(btVector3 &vr, const btVector3 &va,const btVector3 &vb, btScalar blend_factor)
+{
+ vr = (1-blend_factor)*va + blend_factor*vb;
+}
+
+//! This function calcs the distance from a 3D plane
+SIMD_FORCE_INLINE void bt_plane_clip_polygon_collect(
+ const btVector3 & point0,
+ const btVector3 & point1,
+ btScalar dist0,
+ btScalar dist1,
+ btVector3 * clipped,
+ int & clipped_count)
+{
+ bool _prevclassif = (dist0>SIMD_EPSILON);
+ bool _classif = (dist1>SIMD_EPSILON);
+ if(_classif!=_prevclassif)
+ {
+ btScalar blendfactor = -dist0/(dist1-dist0);
+ bt_vec_blend(clipped[clipped_count],point0,point1,blendfactor);
+ clipped_count++;
+ }
+ if(!_classif)
+ {
+ clipped[clipped_count] = point1;
+ clipped_count++;
+ }
+}
+
+
+//! Clips a polygon by a plane
+/*!
+*\return The count of the clipped counts
+*/
+SIMD_FORCE_INLINE int bt_plane_clip_polygon(
+ const btVector4 & plane,
+ const btVector3 * polygon_points,
+ int polygon_point_count,
+ btVector3 * clipped)
+{
+ int clipped_count = 0;
+
+
+ //clip first point
+ btScalar firstdist = bt_distance_point_plane(plane,polygon_points[0]);;
+ if(!(firstdist>SIMD_EPSILON))
+ {
+ clipped[clipped_count] = polygon_points[0];
+ clipped_count++;
+ }
+
+ btScalar olddist = firstdist;
+ for(int i=1;i<polygon_point_count;i++)
+ {
+ btScalar dist = bt_distance_point_plane(plane,polygon_points[i]);
+
+ bt_plane_clip_polygon_collect(
+ polygon_points[i-1],polygon_points[i],
+ olddist,
+ dist,
+ clipped,
+ clipped_count);
+
+
+ olddist = dist;
+ }
+
+ //RETURN TO FIRST point
+
+ bt_plane_clip_polygon_collect(
+ polygon_points[polygon_point_count-1],polygon_points[0],
+ olddist,
+ firstdist,
+ clipped,
+ clipped_count);
+
+ return clipped_count;
+}
+
+//! Clips a polygon by a plane
+/*!
+*\param clipped must be an array of 16 points.
+*\return The count of the clipped counts
+*/
+SIMD_FORCE_INLINE int bt_plane_clip_triangle(
+ const btVector4 & plane,
+ const btVector3 & point0,
+ const btVector3 & point1,
+ const btVector3& point2,
+ btVector3 * clipped // an allocated array of 16 points at least
+ )
+{
+ int clipped_count = 0;
+
+ //clip first point0
+ btScalar firstdist = bt_distance_point_plane(plane,point0);;
+ if(!(firstdist>SIMD_EPSILON))
+ {
+ clipped[clipped_count] = point0;
+ clipped_count++;
+ }
+
+ // point 1
+ btScalar olddist = firstdist;
+ btScalar dist = bt_distance_point_plane(plane,point1);
+
+ bt_plane_clip_polygon_collect(
+ point0,point1,
+ olddist,
+ dist,
+ clipped,
+ clipped_count);
+
+ olddist = dist;
+
+
+ // point 2
+ dist = bt_distance_point_plane(plane,point2);
+
+ bt_plane_clip_polygon_collect(
+ point1,point2,
+ olddist,
+ dist,
+ clipped,
+ clipped_count);
+ olddist = dist;
+
+
+
+ //RETURN TO FIRST point0
+ bt_plane_clip_polygon_collect(
+ point2,point0,
+ olddist,
+ firstdist,
+ clipped,
+ clipped_count);
+
+ return clipped_count;
+}
+
+
+
+
+
+#endif // GIM_TRI_COLLISION_H_INCLUDED
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btContactProcessing.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btContactProcessing.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btContactProcessing.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btContactProcessing.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,145 @@
+#ifndef BT_CONTACT_H_INCLUDED
+#define BT_CONTACT_H_INCLUDED
+
+/*! \file gim_contact.h
+\author Francisco Len Nßjera
+*/
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman at yahoo.com
+
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "btTriangleShapeEx.h"
+
+
+
+/**
+Configuration var for applying interpolation of contact normals
+*/
+#define NORMAL_CONTACT_AVERAGE 1
+
+#define CONTACT_DIFF_EPSILON 0.00001f
+
+///The GIM_CONTACT is an internal GIMPACT structure, similar to btManifoldPoint.
+///@todo: remove and replace GIM_CONTACT by btManifoldPoint.
+class GIM_CONTACT
+{
+public:
+ btVector3 m_point;
+ btVector3 m_normal;
+ btScalar m_depth;//Positive value indicates interpenetration
+ btScalar m_distance;//Padding not for use
+ int m_feature1;//Face number
+ int m_feature2;//Face number
+public:
+ GIM_CONTACT()
+ {
+ }
+
+ GIM_CONTACT(const GIM_CONTACT & contact):
+ m_point(contact.m_point),
+ m_normal(contact.m_normal),
+ m_depth(contact.m_depth),
+ m_feature1(contact.m_feature1),
+ m_feature2(contact.m_feature2)
+ {
+ }
+
+ GIM_CONTACT(const btVector3 &point,const btVector3 & normal,
+ btScalar depth, int feature1, int feature2):
+ m_point(point),
+ m_normal(normal),
+ m_depth(depth),
+ m_feature1(feature1),
+ m_feature2(feature2)
+ {
+ }
+
+ //! Calcs key for coord classification
+ SIMD_FORCE_INLINE unsigned int calc_key_contact() const
+ {
+ int _coords[] = {
+ (int)(m_point[0]*1000.0f+1.0f),
+ (int)(m_point[1]*1333.0f),
+ (int)(m_point[2]*2133.0f+3.0f)};
+ unsigned int _hash=0;
+ unsigned int *_uitmp = (unsigned int *)(&_coords[0]);
+ _hash = *_uitmp;
+ _uitmp++;
+ _hash += (*_uitmp)<<4;
+ _uitmp++;
+ _hash += (*_uitmp)<<8;
+ return _hash;
+ }
+
+ SIMD_FORCE_INLINE void interpolate_normals( btVector3 * normals,int normal_count)
+ {
+ btVector3 vec_sum(m_normal);
+ for(int i=0;i<normal_count;i++)
+ {
+ vec_sum += normals[i];
+ }
+
+ btScalar vec_sum_len = vec_sum.length2();
+ if(vec_sum_len <CONTACT_DIFF_EPSILON) return;
+
+ //GIM_INV_SQRT(vec_sum_len,vec_sum_len); // 1/sqrt(vec_sum_len)
+
+ m_normal = vec_sum/btSqrt(vec_sum_len);
+ }
+
+};
+
+
+class btContactArray:public btAlignedObjectArray<GIM_CONTACT>
+{
+public:
+ btContactArray()
+ {
+ reserve(64);
+ }
+
+ SIMD_FORCE_INLINE void push_contact(
+ const btVector3 &point,const btVector3 & normal,
+ btScalar depth, int feature1, int feature2)
+ {
+ push_back( GIM_CONTACT(point,normal,depth,feature1,feature2) );
+ }
+
+ SIMD_FORCE_INLINE void push_triangle_contacts(
+ const GIM_TRIANGLE_CONTACT & tricontact,
+ int feature1,int feature2)
+ {
+ for(int i = 0;i<tricontact.m_point_count ;i++ )
+ {
+ push_contact(
+ tricontact.m_points[i],
+ tricontact.m_separating_normal,
+ tricontact.m_penetration_depth,feature1,feature2);
+ }
+ }
+
+ void merge_contacts(const btContactArray & contacts, bool normal_contact_average = true);
+
+ void merge_contacts_unique(const btContactArray & contacts);
+};
+
+
+#endif // GIM_CONTACT_H_INCLUDED
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btGImpactBvh.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btGImpactBvh.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btGImpactBvh.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btGImpactBvh.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,396 @@
+#ifndef GIM_BOX_SET_H_INCLUDED
+#define GIM_BOX_SET_H_INCLUDED
+
+/*! \file gim_box_set.h
+\author Francisco Len Nßjera
+*/
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman at yahoo.com
+
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "LinearMath/btAlignedObjectArray.h"
+
+#include "btBoxCollision.h"
+#include "btTriangleShapeEx.h"
+
+
+
+
+
+//! Overlapping pair
+struct GIM_PAIR
+{
+ int m_index1;
+ int m_index2;
+ GIM_PAIR()
+ {}
+
+ GIM_PAIR(const GIM_PAIR & p)
+ {
+ m_index1 = p.m_index1;
+ m_index2 = p.m_index2;
+ }
+
+ GIM_PAIR(int index1, int index2)
+ {
+ m_index1 = index1;
+ m_index2 = index2;
+ }
+};
+
+//! A pairset array
+class btPairSet: public btAlignedObjectArray<GIM_PAIR>
+{
+public:
+ btPairSet()
+ {
+ reserve(32);
+ }
+ inline void push_pair(int index1,int index2)
+ {
+ push_back(GIM_PAIR(index1,index2));
+ }
+
+ inline void push_pair_inv(int index1,int index2)
+ {
+ push_back(GIM_PAIR(index2,index1));
+ }
+};
+
+
+///GIM_BVH_DATA is an internal GIMPACT collision structure to contain axis aligned bounding box
+struct GIM_BVH_DATA
+{
+ btAABB m_bound;
+ int m_data;
+};
+
+//! Node Structure for trees
+class GIM_BVH_TREE_NODE
+{
+public:
+ btAABB m_bound;
+protected:
+ int m_escapeIndexOrDataIndex;
+public:
+ GIM_BVH_TREE_NODE()
+ {
+ m_escapeIndexOrDataIndex = 0;
+ }
+
+ SIMD_FORCE_INLINE bool isLeafNode() const
+ {
+ //skipindex is negative (internal node), triangleindex >=0 (leafnode)
+ return (m_escapeIndexOrDataIndex>=0);
+ }
+
+ SIMD_FORCE_INLINE int getEscapeIndex() const
+ {
+ //btAssert(m_escapeIndexOrDataIndex < 0);
+ return -m_escapeIndexOrDataIndex;
+ }
+
+ SIMD_FORCE_INLINE void setEscapeIndex(int index)
+ {
+ m_escapeIndexOrDataIndex = -index;
+ }
+
+ SIMD_FORCE_INLINE int getDataIndex() const
+ {
+ //btAssert(m_escapeIndexOrDataIndex >= 0);
+
+ return m_escapeIndexOrDataIndex;
+ }
+
+ SIMD_FORCE_INLINE void setDataIndex(int index)
+ {
+ m_escapeIndexOrDataIndex = index;
+ }
+
+};
+
+
+class GIM_BVH_DATA_ARRAY:public btAlignedObjectArray<GIM_BVH_DATA>
+{
+};
+
+
+class GIM_BVH_TREE_NODE_ARRAY:public btAlignedObjectArray<GIM_BVH_TREE_NODE>
+{
+};
+
+
+
+
+//! Basic Box tree structure
+class btBvhTree
+{
+protected:
+ int m_num_nodes;
+ GIM_BVH_TREE_NODE_ARRAY m_node_array;
+protected:
+ int _sort_and_calc_splitting_index(
+ GIM_BVH_DATA_ARRAY & primitive_boxes,
+ int startIndex, int endIndex, int splitAxis);
+
+ int _calc_splitting_axis(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex);
+
+ void _build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex);
+public:
+ btBvhTree()
+ {
+ m_num_nodes = 0;
+ }
+
+ //! prototype functions for box tree management
+ //!@{
+ void build_tree(GIM_BVH_DATA_ARRAY & primitive_boxes);
+
+ SIMD_FORCE_INLINE void clearNodes()
+ {
+ m_node_array.clear();
+ m_num_nodes = 0;
+ }
+
+ //! node count
+ SIMD_FORCE_INLINE int getNodeCount() const
+ {
+ return m_num_nodes;
+ }
+
+ //! tells if the node is a leaf
+ SIMD_FORCE_INLINE bool isLeafNode(int nodeindex) const
+ {
+ return m_node_array[nodeindex].isLeafNode();
+ }
+
+ SIMD_FORCE_INLINE int getNodeData(int nodeindex) const
+ {
+ return m_node_array[nodeindex].getDataIndex();
+ }
+
+ SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB & bound) const
+ {
+ bound = m_node_array[nodeindex].m_bound;
+ }
+
+ SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB & bound)
+ {
+ m_node_array[nodeindex].m_bound = bound;
+ }
+
+ SIMD_FORCE_INLINE int getLeftNode(int nodeindex) const
+ {
+ return nodeindex+1;
+ }
+
+ SIMD_FORCE_INLINE int getRightNode(int nodeindex) const
+ {
+ if(m_node_array[nodeindex+1].isLeafNode()) return nodeindex+2;
+ return nodeindex+1 + m_node_array[nodeindex+1].getEscapeIndex();
+ }
+
+ SIMD_FORCE_INLINE int getEscapeNodeIndex(int nodeindex) const
+ {
+ return m_node_array[nodeindex].getEscapeIndex();
+ }
+
+ SIMD_FORCE_INLINE const GIM_BVH_TREE_NODE * get_node_pointer(int index = 0) const
+ {
+ return &m_node_array[index];
+ }
+
+ //!@}
+};
+
+
+//! Prototype Base class for primitive classification
+/*!
+This class is a wrapper for primitive collections.
+This tells relevant info for the Bounding Box set classes, which take care of space classification.
+This class can manage Compound shapes and trimeshes, and if it is managing trimesh then the Hierarchy Bounding Box classes will take advantage of primitive Vs Box overlapping tests for getting optimal results and less Per Box compairisons.
+*/
+class btPrimitiveManagerBase
+{
+public:
+
+ virtual ~btPrimitiveManagerBase() {}
+
+ //! determines if this manager consist on only triangles, which special case will be optimized
+ virtual bool is_trimesh() const = 0;
+ virtual int get_primitive_count() const = 0;
+ virtual void get_primitive_box(int prim_index ,btAABB & primbox) const = 0;
+ //! retrieves only the points of the triangle, and the collision margin
+ virtual void get_primitive_triangle(int prim_index,btPrimitiveTriangle & triangle) const= 0;
+};
+
+
+//! Structure for containing Boxes
+/*!
+This class offers an structure for managing a box tree of primitives.
+Requires a Primitive prototype (like btPrimitiveManagerBase )
+*/
+class btGImpactBvh
+{
+protected:
+ btBvhTree m_box_tree;
+ btPrimitiveManagerBase * m_primitive_manager;
+
+protected:
+ //stackless refit
+ void refit();
+public:
+
+ //! this constructor doesn't build the tree. you must call buildSet
+ btGImpactBvh()
+ {
+ m_primitive_manager = NULL;
+ }
+
+ //! this constructor doesn't build the tree. you must call buildSet
+ btGImpactBvh(btPrimitiveManagerBase * primitive_manager)
+ {
+ m_primitive_manager = primitive_manager;
+ }
+
+ SIMD_FORCE_INLINE btAABB getGlobalBox() const
+ {
+ btAABB totalbox;
+ getNodeBound(0, totalbox);
+ return totalbox;
+ }
+
+ SIMD_FORCE_INLINE void setPrimitiveManager(btPrimitiveManagerBase * primitive_manager)
+ {
+ m_primitive_manager = primitive_manager;
+ }
+
+ SIMD_FORCE_INLINE btPrimitiveManagerBase * getPrimitiveManager() const
+ {
+ return m_primitive_manager;
+ }
+
+
+//! node manager prototype functions
+///@{
+
+ //! this attemps to refit the box set.
+ SIMD_FORCE_INLINE void update()
+ {
+ refit();
+ }
+
+ //! this rebuild the entire set
+ void buildSet();
+
+ //! returns the indices of the primitives in the m_primitive_manager
+ bool boxQuery(const btAABB & box, btAlignedObjectArray<int> & collided_results) const;
+
+ //! returns the indices of the primitives in the m_primitive_manager
+ SIMD_FORCE_INLINE bool boxQueryTrans(const btAABB & box,
+ const btTransform & transform, btAlignedObjectArray<int> & collided_results) const
+ {
+ btAABB transbox=box;
+ transbox.appy_transform(transform);
+ return boxQuery(transbox,collided_results);
+ }
+
+ //! returns the indices of the primitives in the m_primitive_manager
+ bool rayQuery(
+ const btVector3 & ray_dir,const btVector3 & ray_origin ,
+ btAlignedObjectArray<int> & collided_results) const;
+
+ //! tells if this set has hierarcht
+ SIMD_FORCE_INLINE bool hasHierarchy() const
+ {
+ return true;
+ }
+
+ //! tells if this set is a trimesh
+ SIMD_FORCE_INLINE bool isTrimesh() const
+ {
+ return m_primitive_manager->is_trimesh();
+ }
+
+ //! node count
+ SIMD_FORCE_INLINE int getNodeCount() const
+ {
+ return m_box_tree.getNodeCount();
+ }
+
+ //! tells if the node is a leaf
+ SIMD_FORCE_INLINE bool isLeafNode(int nodeindex) const
+ {
+ return m_box_tree.isLeafNode(nodeindex);
+ }
+
+ SIMD_FORCE_INLINE int getNodeData(int nodeindex) const
+ {
+ return m_box_tree.getNodeData(nodeindex);
+ }
+
+ SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB & bound) const
+ {
+ m_box_tree.getNodeBound(nodeindex, bound);
+ }
+
+ SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB & bound)
+ {
+ m_box_tree.setNodeBound(nodeindex, bound);
+ }
+
+
+ SIMD_FORCE_INLINE int getLeftNode(int nodeindex) const
+ {
+ return m_box_tree.getLeftNode(nodeindex);
+ }
+
+ SIMD_FORCE_INLINE int getRightNode(int nodeindex) const
+ {
+ return m_box_tree.getRightNode(nodeindex);
+ }
+
+ SIMD_FORCE_INLINE int getEscapeNodeIndex(int nodeindex) const
+ {
+ return m_box_tree.getEscapeNodeIndex(nodeindex);
+ }
+
+ SIMD_FORCE_INLINE void getNodeTriangle(int nodeindex,btPrimitiveTriangle & triangle) const
+ {
+ m_primitive_manager->get_primitive_triangle(getNodeData(nodeindex),triangle);
+ }
+
+
+ SIMD_FORCE_INLINE const GIM_BVH_TREE_NODE * get_node_pointer(int index = 0) const
+ {
+ return m_box_tree.get_node_pointer(index);
+ }
+
+
+ static float getAverageTreeCollisionTime();
+
+
+ static void find_collision(btGImpactBvh * boxset1, const btTransform & trans1,
+ btGImpactBvh * boxset2, const btTransform & trans2,
+ btPairSet & collision_pairs);
+};
+
+
+#endif // GIM_BOXPRUNING_H_INCLUDED
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,306 @@
+/*! \file btGImpactShape.h
+\author Francisco Len Nßjera
+*/
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman at yahoo.com
+
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BVH_CONCAVE_COLLISION_ALGORITHM_H
+#define BVH_CONCAVE_COLLISION_ALGORITHM_H
+
+#include "BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h"
+#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+class btDispatcher;
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+
+#include "LinearMath/btAlignedObjectArray.h"
+
+#include "btGImpactShape.h"
+#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
+#include "BulletCollision/CollisionShapes/btCompoundShape.h"
+#include "BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h"
+#include "LinearMath/btIDebugDraw.h"
+
+
+
+//! Collision Algorithm for GImpact Shapes
+/*!
+For register this algorithm in Bullet, proceed as following:
+ \code
+btCollisionDispatcher * dispatcher = static_cast<btCollisionDispatcher *>(m_dynamicsWorld ->getDispatcher());
+btGImpactCollisionAlgorithm::registerAlgorithm(dispatcher);
+ \endcode
+*/
+class btGImpactCollisionAlgorithm : public btActivatingCollisionAlgorithm
+{
+protected:
+ btCollisionAlgorithm * m_convex_algorithm;
+ btPersistentManifold * m_manifoldPtr;
+ btManifoldResult* m_resultOut;
+ const btDispatcherInfo * m_dispatchInfo;
+ int m_triface0;
+ int m_part0;
+ int m_triface1;
+ int m_part1;
+
+
+ //! Creates a new contact point
+ SIMD_FORCE_INLINE btPersistentManifold* newContactManifold(btCollisionObject* body0,btCollisionObject* body1)
+ {
+ m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1);
+ return m_manifoldPtr;
+ }
+
+ SIMD_FORCE_INLINE void destroyConvexAlgorithm()
+ {
+ if(m_convex_algorithm)
+ {
+ m_convex_algorithm->~btCollisionAlgorithm();
+ m_dispatcher->freeCollisionAlgorithm( m_convex_algorithm);
+ m_convex_algorithm = NULL;
+ }
+ }
+
+ SIMD_FORCE_INLINE void destroyContactManifolds()
+ {
+ if(m_manifoldPtr == NULL) return;
+ m_dispatcher->releaseManifold(m_manifoldPtr);
+ m_manifoldPtr = NULL;
+ }
+
+ SIMD_FORCE_INLINE void clearCache()
+ {
+ destroyContactManifolds();
+ destroyConvexAlgorithm();
+
+ m_triface0 = -1;
+ m_part0 = -1;
+ m_triface1 = -1;
+ m_part1 = -1;
+ }
+
+ SIMD_FORCE_INLINE btPersistentManifold* getLastManifold()
+ {
+ return m_manifoldPtr;
+ }
+
+
+ // Call before process collision
+ SIMD_FORCE_INLINE void checkManifold(btCollisionObject* body0,btCollisionObject* body1)
+ {
+ if(getLastManifold() == 0)
+ {
+ newContactManifold(body0,body1);
+ }
+
+ m_resultOut->setPersistentManifold(getLastManifold());
+ }
+
+ // Call before process collision
+ SIMD_FORCE_INLINE btCollisionAlgorithm * newAlgorithm(btCollisionObject* body0,btCollisionObject* body1)
+ {
+ checkManifold(body0,body1);
+
+ btCollisionAlgorithm * convex_algorithm = m_dispatcher->findAlgorithm(
+ body0,body1,getLastManifold());
+ return convex_algorithm ;
+ }
+
+ // Call before process collision
+ SIMD_FORCE_INLINE void checkConvexAlgorithm(btCollisionObject* body0,btCollisionObject* body1)
+ {
+ if(m_convex_algorithm) return;
+ m_convex_algorithm = newAlgorithm(body0,body1);
+ }
+
+
+
+
+ void addContactPoint(btCollisionObject * body0,
+ btCollisionObject * body1,
+ const btVector3 & point,
+ const btVector3 & normal,
+ btScalar distance);
+
+//! Collision routines
+//!@{
+
+ void collide_gjk_triangles(btCollisionObject * body0,
+ btCollisionObject * body1,
+ btGImpactMeshShapePart * shape0,
+ btGImpactMeshShapePart * shape1,
+ const int * pairs, int pair_count);
+
+ void collide_sat_triangles(btCollisionObject * body0,
+ btCollisionObject * body1,
+ btGImpactMeshShapePart * shape0,
+ btGImpactMeshShapePart * shape1,
+ const int * pairs, int pair_count);
+
+
+
+
+ void shape_vs_shape_collision(
+ btCollisionObject * body0,
+ btCollisionObject * body1,
+ btCollisionShape * shape0,
+ btCollisionShape * shape1);
+
+ void convex_vs_convex_collision(btCollisionObject * body0,
+ btCollisionObject * body1,
+ btCollisionShape * shape0,
+ btCollisionShape * shape1);
+
+
+
+ void gimpact_vs_gimpact_find_pairs(
+ const btTransform & trans0,
+ const btTransform & trans1,
+ btGImpactShapeInterface * shape0,
+ btGImpactShapeInterface * shape1,btPairSet & pairset);
+
+ void gimpact_vs_shape_find_pairs(
+ const btTransform & trans0,
+ const btTransform & trans1,
+ btGImpactShapeInterface * shape0,
+ btCollisionShape * shape1,
+ btAlignedObjectArray<int> & collided_primitives);
+
+
+ void gimpacttrimeshpart_vs_plane_collision(
+ btCollisionObject * body0,
+ btCollisionObject * body1,
+ btGImpactMeshShapePart * shape0,
+ btStaticPlaneShape * shape1,bool swapped);
+
+
+public:
+
+ btGImpactCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1);
+
+ virtual ~btGImpactCollisionAlgorithm();
+
+ virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+ btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+ virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
+ {
+ if (m_manifoldPtr)
+ manifoldArray.push_back(m_manifoldPtr);
+ }
+
+
+ struct CreateFunc :public btCollisionAlgorithmCreateFunc
+ {
+ virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+ {
+ void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btGImpactCollisionAlgorithm));
+ return new(mem) btGImpactCollisionAlgorithm(ci,body0,body1);
+ }
+ };
+
+ //! Use this function for register the algorithm externally
+ static void registerAlgorithm(btCollisionDispatcher * dispatcher);
+
+ //! Gets the average time in miliseconds of tree collisions
+ static float getAverageTreeCollisionTime();
+
+ //! Gets the average time in miliseconds of triangle collisions
+ static float getAverageTriangleCollisionTime();
+
+
+ //! Collides two gimpact shapes
+ /*!
+ \pre shape0 and shape1 couldn't be btGImpactMeshShape objects
+ */
+
+
+ void gimpact_vs_gimpact(btCollisionObject * body0,
+ btCollisionObject * body1,
+ btGImpactShapeInterface * shape0,
+ btGImpactShapeInterface * shape1);
+
+ void gimpact_vs_shape(btCollisionObject * body0,
+ btCollisionObject * body1,
+ btGImpactShapeInterface * shape0,
+ btCollisionShape * shape1,bool swapped);
+
+ void gimpact_vs_compoundshape(btCollisionObject * body0,
+ btCollisionObject * body1,
+ btGImpactShapeInterface * shape0,
+ btCompoundShape * shape1,bool swapped);
+
+ void gimpact_vs_concave(
+ btCollisionObject * body0,
+ btCollisionObject * body1,
+ btGImpactShapeInterface * shape0,
+ btConcaveShape * shape1,bool swapped);
+
+
+
+
+ /// Accessor/Mutator pairs for Part and triangleID
+ void setFace0(int value)
+ {
+ m_triface0 = value;
+ }
+ int getFace0()
+ {
+ return m_triface0;
+ }
+ void setFace1(int value)
+ {
+ m_triface1 = value;
+ }
+ int getFace1()
+ {
+ return m_triface1;
+ }
+ void setPart0(int value)
+ {
+ m_part0 = value;
+ }
+ int getPart0()
+ {
+ return m_part0;
+ }
+ void setPart1(int value)
+ {
+ m_part1 = value;
+ }
+ int getPart1()
+ {
+ return m_part1;
+ }
+
+};
+
+
+//algorithm details
+//#define BULLET_TRIANGLE_COLLISION 1
+#define GIMPACT_VS_PLANE_COLLISION 1
+
+
+
+#endif //BVH_CONCAVE_COLLISION_ALGORITHM_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btGImpactMassUtil.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btGImpactMassUtil.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btGImpactMassUtil.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btGImpactMassUtil.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,60 @@
+/*! \file btGImpactMassUtil.h
+\author Francisco Len Nßjera
+*/
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman at yahoo.com
+
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef GIMPACT_MASS_UTIL_H
+#define GIMPACT_MASS_UTIL_H
+
+#include "LinearMath/btTransform.h"
+
+
+
+SIMD_FORCE_INLINE btVector3 gim_inertia_add_transformed(
+ const btVector3 & source_inertia, const btVector3 & added_inertia, const btTransform & transform)
+{
+ btMatrix3x3 rotatedTensor = transform.getBasis().scaled(added_inertia) * transform.getBasis().transpose();
+
+ btScalar x2 = transform.getOrigin()[0];
+ x2*= x2;
+ btScalar y2 = transform.getOrigin()[1];
+ y2*= y2;
+ btScalar z2 = transform.getOrigin()[2];
+ z2*= z2;
+
+ btScalar ix = rotatedTensor[0][0]*(y2+z2);
+ btScalar iy = rotatedTensor[1][1]*(x2+z2);
+ btScalar iz = rotatedTensor[2][2]*(x2+y2);
+
+ return btVector3(source_inertia[0]+ix,source_inertia[1]+iy,source_inertia[2] + iz);
+}
+
+SIMD_FORCE_INLINE btVector3 gim_get_point_inertia(const btVector3 & point, btScalar mass)
+{
+ btScalar x2 = point[0]*point[0];
+ btScalar y2 = point[1]*point[1];
+ btScalar z2 = point[2]*point[2];
+ return btVector3(mass*(y2+z2),mass*(x2+z2),mass*(x2+y2));
+}
+
+
+#endif //GIMPACT_MESH_SHAPE_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btGImpactQuantizedBvh.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btGImpactQuantizedBvh.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btGImpactQuantizedBvh.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btGImpactQuantizedBvh.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,372 @@
+#ifndef GIM_QUANTIZED_SET_H_INCLUDED
+#define GIM_QUANTIZED_SET_H_INCLUDED
+
+/*! \file btGImpactQuantizedBvh.h
+\author Francisco Len Nßjera
+*/
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman at yahoo.com
+
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btGImpactBvh.h"
+#include "btQuantization.h"
+
+
+
+
+
+///btQuantizedBvhNode is a compressed aabb node, 16 bytes.
+///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range).
+ATTRIBUTE_ALIGNED16 (struct) BT_QUANTIZED_BVH_NODE
+{
+ //12 bytes
+ unsigned short int m_quantizedAabbMin[3];
+ unsigned short int m_quantizedAabbMax[3];
+ //4 bytes
+ int m_escapeIndexOrDataIndex;
+
+ BT_QUANTIZED_BVH_NODE()
+ {
+ m_escapeIndexOrDataIndex = 0;
+ }
+
+ SIMD_FORCE_INLINE bool isLeafNode() const
+ {
+ //skipindex is negative (internal node), triangleindex >=0 (leafnode)
+ return (m_escapeIndexOrDataIndex>=0);
+ }
+
+ SIMD_FORCE_INLINE int getEscapeIndex() const
+ {
+ //btAssert(m_escapeIndexOrDataIndex < 0);
+ return -m_escapeIndexOrDataIndex;
+ }
+
+ SIMD_FORCE_INLINE void setEscapeIndex(int index)
+ {
+ m_escapeIndexOrDataIndex = -index;
+ }
+
+ SIMD_FORCE_INLINE int getDataIndex() const
+ {
+ //btAssert(m_escapeIndexOrDataIndex >= 0);
+
+ return m_escapeIndexOrDataIndex;
+ }
+
+ SIMD_FORCE_INLINE void setDataIndex(int index)
+ {
+ m_escapeIndexOrDataIndex = index;
+ }
+
+ SIMD_FORCE_INLINE bool testQuantizedBoxOverlapp(
+ unsigned short * quantizedMin,unsigned short * quantizedMax) const
+ {
+ if(m_quantizedAabbMin[0] > quantizedMax[0] ||
+ m_quantizedAabbMax[0] < quantizedMin[0] ||
+ m_quantizedAabbMin[1] > quantizedMax[1] ||
+ m_quantizedAabbMax[1] < quantizedMin[1] ||
+ m_quantizedAabbMin[2] > quantizedMax[2] ||
+ m_quantizedAabbMax[2] < quantizedMin[2])
+ {
+ return false;
+ }
+ return true;
+ }
+
+};
+
+
+
+class GIM_QUANTIZED_BVH_NODE_ARRAY:public btAlignedObjectArray<BT_QUANTIZED_BVH_NODE>
+{
+};
+
+
+
+
+//! Basic Box tree structure
+class btQuantizedBvhTree
+{
+protected:
+ int m_num_nodes;
+ GIM_QUANTIZED_BVH_NODE_ARRAY m_node_array;
+ btAABB m_global_bound;
+ btVector3 m_bvhQuantization;
+protected:
+ void calc_quantization(GIM_BVH_DATA_ARRAY & primitive_boxes, btScalar boundMargin = btScalar(1.0) );
+
+ int _sort_and_calc_splitting_index(
+ GIM_BVH_DATA_ARRAY & primitive_boxes,
+ int startIndex, int endIndex, int splitAxis);
+
+ int _calc_splitting_axis(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex);
+
+ void _build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex);
+public:
+ btQuantizedBvhTree()
+ {
+ m_num_nodes = 0;
+ }
+
+ //! prototype functions for box tree management
+ //!@{
+ void build_tree(GIM_BVH_DATA_ARRAY & primitive_boxes);
+
+ SIMD_FORCE_INLINE void quantizePoint(
+ unsigned short * quantizedpoint, const btVector3 & point) const
+ {
+ bt_quantize_clamp(quantizedpoint,point,m_global_bound.m_min,m_global_bound.m_max,m_bvhQuantization);
+ }
+
+
+ SIMD_FORCE_INLINE bool testQuantizedBoxOverlapp(
+ int node_index,
+ unsigned short * quantizedMin,unsigned short * quantizedMax) const
+ {
+ return m_node_array[node_index].testQuantizedBoxOverlapp(quantizedMin,quantizedMax);
+ }
+
+ SIMD_FORCE_INLINE void clearNodes()
+ {
+ m_node_array.clear();
+ m_num_nodes = 0;
+ }
+
+ //! node count
+ SIMD_FORCE_INLINE int getNodeCount() const
+ {
+ return m_num_nodes;
+ }
+
+ //! tells if the node is a leaf
+ SIMD_FORCE_INLINE bool isLeafNode(int nodeindex) const
+ {
+ return m_node_array[nodeindex].isLeafNode();
+ }
+
+ SIMD_FORCE_INLINE int getNodeData(int nodeindex) const
+ {
+ return m_node_array[nodeindex].getDataIndex();
+ }
+
+ SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB & bound) const
+ {
+ bound.m_min = bt_unquantize(
+ m_node_array[nodeindex].m_quantizedAabbMin,
+ m_global_bound.m_min,m_bvhQuantization);
+
+ bound.m_max = bt_unquantize(
+ m_node_array[nodeindex].m_quantizedAabbMax,
+ m_global_bound.m_min,m_bvhQuantization);
+ }
+
+ SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB & bound)
+ {
+ bt_quantize_clamp( m_node_array[nodeindex].m_quantizedAabbMin,
+ bound.m_min,
+ m_global_bound.m_min,
+ m_global_bound.m_max,
+ m_bvhQuantization);
+
+ bt_quantize_clamp( m_node_array[nodeindex].m_quantizedAabbMax,
+ bound.m_max,
+ m_global_bound.m_min,
+ m_global_bound.m_max,
+ m_bvhQuantization);
+ }
+
+ SIMD_FORCE_INLINE int getLeftNode(int nodeindex) const
+ {
+ return nodeindex+1;
+ }
+
+ SIMD_FORCE_INLINE int getRightNode(int nodeindex) const
+ {
+ if(m_node_array[nodeindex+1].isLeafNode()) return nodeindex+2;
+ return nodeindex+1 + m_node_array[nodeindex+1].getEscapeIndex();
+ }
+
+ SIMD_FORCE_INLINE int getEscapeNodeIndex(int nodeindex) const
+ {
+ return m_node_array[nodeindex].getEscapeIndex();
+ }
+
+ SIMD_FORCE_INLINE const BT_QUANTIZED_BVH_NODE * get_node_pointer(int index = 0) const
+ {
+ return &m_node_array[index];
+ }
+
+ //!@}
+};
+
+
+
+//! Structure for containing Boxes
+/*!
+This class offers an structure for managing a box tree of primitives.
+Requires a Primitive prototype (like btPrimitiveManagerBase )
+*/
+class btGImpactQuantizedBvh
+{
+protected:
+ btQuantizedBvhTree m_box_tree;
+ btPrimitiveManagerBase * m_primitive_manager;
+
+protected:
+ //stackless refit
+ void refit();
+public:
+
+ //! this constructor doesn't build the tree. you must call buildSet
+ btGImpactQuantizedBvh()
+ {
+ m_primitive_manager = NULL;
+ }
+
+ //! this constructor doesn't build the tree. you must call buildSet
+ btGImpactQuantizedBvh(btPrimitiveManagerBase * primitive_manager)
+ {
+ m_primitive_manager = primitive_manager;
+ }
+
+ SIMD_FORCE_INLINE btAABB getGlobalBox() const
+ {
+ btAABB totalbox;
+ getNodeBound(0, totalbox);
+ return totalbox;
+ }
+
+ SIMD_FORCE_INLINE void setPrimitiveManager(btPrimitiveManagerBase * primitive_manager)
+ {
+ m_primitive_manager = primitive_manager;
+ }
+
+ SIMD_FORCE_INLINE btPrimitiveManagerBase * getPrimitiveManager() const
+ {
+ return m_primitive_manager;
+ }
+
+
+//! node manager prototype functions
+///@{
+
+ //! this attemps to refit the box set.
+ SIMD_FORCE_INLINE void update()
+ {
+ refit();
+ }
+
+ //! this rebuild the entire set
+ void buildSet();
+
+ //! returns the indices of the primitives in the m_primitive_manager
+ bool boxQuery(const btAABB & box, btAlignedObjectArray<int> & collided_results) const;
+
+ //! returns the indices of the primitives in the m_primitive_manager
+ SIMD_FORCE_INLINE bool boxQueryTrans(const btAABB & box,
+ const btTransform & transform, btAlignedObjectArray<int> & collided_results) const
+ {
+ btAABB transbox=box;
+ transbox.appy_transform(transform);
+ return boxQuery(transbox,collided_results);
+ }
+
+ //! returns the indices of the primitives in the m_primitive_manager
+ bool rayQuery(
+ const btVector3 & ray_dir,const btVector3 & ray_origin ,
+ btAlignedObjectArray<int> & collided_results) const;
+
+ //! tells if this set has hierarcht
+ SIMD_FORCE_INLINE bool hasHierarchy() const
+ {
+ return true;
+ }
+
+ //! tells if this set is a trimesh
+ SIMD_FORCE_INLINE bool isTrimesh() const
+ {
+ return m_primitive_manager->is_trimesh();
+ }
+
+ //! node count
+ SIMD_FORCE_INLINE int getNodeCount() const
+ {
+ return m_box_tree.getNodeCount();
+ }
+
+ //! tells if the node is a leaf
+ SIMD_FORCE_INLINE bool isLeafNode(int nodeindex) const
+ {
+ return m_box_tree.isLeafNode(nodeindex);
+ }
+
+ SIMD_FORCE_INLINE int getNodeData(int nodeindex) const
+ {
+ return m_box_tree.getNodeData(nodeindex);
+ }
+
+ SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB & bound) const
+ {
+ m_box_tree.getNodeBound(nodeindex, bound);
+ }
+
+ SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB & bound)
+ {
+ m_box_tree.setNodeBound(nodeindex, bound);
+ }
+
+
+ SIMD_FORCE_INLINE int getLeftNode(int nodeindex) const
+ {
+ return m_box_tree.getLeftNode(nodeindex);
+ }
+
+ SIMD_FORCE_INLINE int getRightNode(int nodeindex) const
+ {
+ return m_box_tree.getRightNode(nodeindex);
+ }
+
+ SIMD_FORCE_INLINE int getEscapeNodeIndex(int nodeindex) const
+ {
+ return m_box_tree.getEscapeNodeIndex(nodeindex);
+ }
+
+ SIMD_FORCE_INLINE void getNodeTriangle(int nodeindex,btPrimitiveTriangle & triangle) const
+ {
+ m_primitive_manager->get_primitive_triangle(getNodeData(nodeindex),triangle);
+ }
+
+
+ SIMD_FORCE_INLINE const BT_QUANTIZED_BVH_NODE * get_node_pointer(int index = 0) const
+ {
+ return m_box_tree.get_node_pointer(index);
+ }
+
+
+ static float getAverageTreeCollisionTime();
+
+
+ static void find_collision(btGImpactQuantizedBvh * boxset1, const btTransform & trans1,
+ btGImpactQuantizedBvh * boxset2, const btTransform & trans2,
+ btPairSet & collision_pairs);
+};
+
+
+#endif // GIM_BOXPRUNING_H_INCLUDED
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btGImpactShape.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btGImpactShape.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btGImpactShape.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btGImpactShape.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,1124 @@
+/*! \file btGImpactShape.h
+\author Francisco Len Nßjera
+*/
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman at yahoo.com
+
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef GIMPACT_SHAPE_H
+#define GIMPACT_SHAPE_H
+
+#include "BulletCollision/CollisionShapes/btCollisionShape.h"
+#include "BulletCollision/CollisionShapes/btTriangleShape.h"
+#include "BulletCollision/CollisionShapes/btStridingMeshInterface.h"
+#include "BulletCollision/CollisionShapes/btCollisionMargin.h"
+#include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
+#include "BulletCollision/CollisionShapes/btConcaveShape.h"
+#include "BulletCollision/CollisionShapes/btTetrahedronShape.h"
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btMatrix3x3.h"
+#include "LinearMath/btAlignedObjectArray.h"
+
+#include "btGImpactQuantizedBvh.h" // box tree class
+
+
+//! declare Quantized trees, (you can change to float based trees)
+typedef btGImpactQuantizedBvh btGImpactBoxSet;
+
+enum eGIMPACT_SHAPE_TYPE
+{
+ CONST_GIMPACT_COMPOUND_SHAPE = 0,
+ CONST_GIMPACT_TRIMESH_SHAPE_PART,
+ CONST_GIMPACT_TRIMESH_SHAPE
+};
+
+
+//! Helper class for tetrahedrons
+class btTetrahedronShapeEx:public btBU_Simplex1to4
+{
+public:
+ btTetrahedronShapeEx()
+ {
+ m_numVertices = 4;
+ }
+
+
+ SIMD_FORCE_INLINE void setVertices(
+ const btVector3 & v0,const btVector3 & v1,
+ const btVector3 & v2,const btVector3 & v3)
+ {
+ m_vertices[0] = v0;
+ m_vertices[1] = v1;
+ m_vertices[2] = v2;
+ m_vertices[3] = v3;
+ recalcLocalAabb();
+ }
+};
+
+
+//! Base class for gimpact shapes
+class btGImpactShapeInterface : public btConcaveShape
+{
+protected:
+ btAABB m_localAABB;
+ bool m_needs_update;
+ btVector3 localScaling;
+ btGImpactBoxSet m_box_set;// optionally boxset
+
+ //! use this function for perfofm refit in bounding boxes
+ //! use this function for perfofm refit in bounding boxes
+ virtual void calcLocalAABB()
+ {
+ lockChildShapes();
+ if(m_box_set.getNodeCount() == 0)
+ {
+ m_box_set.buildSet();
+ }
+ else
+ {
+ m_box_set.update();
+ }
+ unlockChildShapes();
+
+ m_localAABB = m_box_set.getGlobalBox();
+ }
+
+
+public:
+ btGImpactShapeInterface()
+ {
+ m_shapeType=GIMPACT_SHAPE_PROXYTYPE;
+ m_localAABB.invalidate();
+ m_needs_update = true;
+ localScaling.setValue(1.f,1.f,1.f);
+ }
+
+
+ //! performs refit operation
+ /*!
+ Updates the entire Box set of this shape.
+ \pre postUpdate() must be called for attemps to calculating the box set, else this function
+ will does nothing.
+ \post if m_needs_update == true, then it calls calcLocalAABB();
+ */
+ SIMD_FORCE_INLINE void updateBound()
+ {
+ if(!m_needs_update) return;
+ calcLocalAABB();
+ m_needs_update = false;
+ }
+
+ //! If the Bounding box is not updated, then this class attemps to calculate it.
+ /*!
+ \post Calls updateBound() for update the box set.
+ */
+ void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
+ {
+ btAABB transformedbox = m_localAABB;
+ transformedbox.appy_transform(t);
+ aabbMin = transformedbox.m_min;
+ aabbMax = transformedbox.m_max;
+ }
+
+ //! Tells to this object that is needed to refit the box set
+ virtual void postUpdate()
+ {
+ m_needs_update = true;
+ }
+
+ //! Obtains the local box, which is the global calculated box of the total of subshapes
+ SIMD_FORCE_INLINE const btAABB & getLocalBox()
+ {
+ return m_localAABB;
+ }
+
+
+ virtual int getShapeType() const
+ {
+ return GIMPACT_SHAPE_PROXYTYPE;
+ }
+
+ /*!
+ \post You must call updateBound() for update the box set.
+ */
+ virtual void setLocalScaling(const btVector3& scaling)
+ {
+ localScaling = scaling;
+ postUpdate();
+ }
+
+ virtual const btVector3& getLocalScaling() const
+ {
+ return localScaling;
+ }
+
+
+ virtual void setMargin(btScalar margin)
+ {
+ m_collisionMargin = margin;
+ int i = getNumChildShapes();
+ while(i--)
+ {
+ btCollisionShape* child = getChildShape(i);
+ child->setMargin(margin);
+ }
+
+ m_needs_update = true;
+ }
+
+
+ //! Subshape member functions
+ //!@{
+
+ //! Base method for determinig which kind of GIMPACT shape we get
+ virtual eGIMPACT_SHAPE_TYPE getGImpactShapeType() = 0;
+
+ //! gets boxset
+ SIMD_FORCE_INLINE btGImpactBoxSet * getBoxSet()
+ {
+ return &m_box_set;
+ }
+
+ //! Determines if this class has a hierarchy structure for sorting its primitives
+ SIMD_FORCE_INLINE bool hasBoxSet() const
+ {
+ if(m_box_set.getNodeCount() == 0) return false;
+ return true;
+ }
+
+ //! Obtains the primitive manager
+ virtual const btPrimitiveManagerBase * getPrimitiveManager() const = 0;
+
+
+ //! Gets the number of children
+ virtual int getNumChildShapes() const = 0;
+
+ //! if true, then its children must get transforms.
+ virtual bool childrenHasTransform() const = 0;
+
+ //! Determines if this shape has triangles
+ virtual bool needsRetrieveTriangles() const = 0;
+
+ //! Determines if this shape has tetrahedrons
+ virtual bool needsRetrieveTetrahedrons() const = 0;
+
+ virtual void getBulletTriangle(int prim_index,btTriangleShapeEx & triangle) const = 0;
+
+ virtual void getBulletTetrahedron(int prim_index,btTetrahedronShapeEx & tetrahedron) const = 0;
+
+
+
+ //! call when reading child shapes
+ virtual void lockChildShapes() const
+ {
+ }
+
+ virtual void unlockChildShapes() const
+ {
+ }
+
+ //! if this trimesh
+ SIMD_FORCE_INLINE void getPrimitiveTriangle(int index,btPrimitiveTriangle & triangle) const
+ {
+ getPrimitiveManager()->get_primitive_triangle(index,triangle);
+ }
+
+
+ //! Retrieves the bound from a child
+ /*!
+ */
+ virtual void getChildAabb(int child_index,const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
+ {
+ btAABB child_aabb;
+ getPrimitiveManager()->get_primitive_box(child_index,child_aabb);
+ child_aabb.appy_transform(t);
+ aabbMin = child_aabb.m_min;
+ aabbMax = child_aabb.m_max;
+ }
+
+ //! Gets the children
+ virtual btCollisionShape* getChildShape(int index) = 0;
+
+
+ //! Gets the child
+ virtual const btCollisionShape* getChildShape(int index) const = 0;
+
+ //! Gets the children transform
+ virtual btTransform getChildTransform(int index) const = 0;
+
+ //! Sets the children transform
+ /*!
+ \post You must call updateBound() for update the box set.
+ */
+ virtual void setChildTransform(int index, const btTransform & transform) = 0;
+
+ //!@}
+
+
+ //! virtual method for ray collision
+ virtual void rayTest(const btVector3& rayFrom, const btVector3& rayTo, btCollisionWorld::RayResultCallback& resultCallback) const
+ {
+ }
+
+ //! Function for retrieve triangles.
+ /*!
+ It gives the triangles in local space
+ */
+ virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
+ {
+ }
+
+ //!@}
+
+};
+
+
+//! btGImpactCompoundShape allows to handle multiple btCollisionShape objects at once
+/*!
+This class only can manage Convex subshapes
+*/
+class btGImpactCompoundShape : public btGImpactShapeInterface
+{
+public:
+ //! compound primitive manager
+ class CompoundPrimitiveManager:public btPrimitiveManagerBase
+ {
+ public:
+ virtual ~CompoundPrimitiveManager() {}
+ btGImpactCompoundShape * m_compoundShape;
+
+
+ CompoundPrimitiveManager(const CompoundPrimitiveManager& compound)
+ {
+ m_compoundShape = compound.m_compoundShape;
+ }
+
+ CompoundPrimitiveManager(btGImpactCompoundShape * compoundShape)
+ {
+ m_compoundShape = compoundShape;
+ }
+
+ CompoundPrimitiveManager()
+ {
+ m_compoundShape = NULL;
+ }
+
+ virtual bool is_trimesh() const
+ {
+ return false;
+ }
+
+ virtual int get_primitive_count() const
+ {
+ return (int )m_compoundShape->getNumChildShapes();
+ }
+
+ virtual void get_primitive_box(int prim_index ,btAABB & primbox) const
+ {
+ btTransform prim_trans;
+ if(m_compoundShape->childrenHasTransform())
+ {
+ prim_trans = m_compoundShape->getChildTransform(prim_index);
+ }
+ else
+ {
+ prim_trans.setIdentity();
+ }
+ const btCollisionShape* shape = m_compoundShape->getChildShape(prim_index);
+ shape->getAabb(prim_trans,primbox.m_min,primbox.m_max);
+ }
+
+ virtual void get_primitive_triangle(int prim_index,btPrimitiveTriangle & triangle) const
+ {
+ btAssert(0);
+ }
+
+ };
+
+
+
+protected:
+ CompoundPrimitiveManager m_primitive_manager;
+ btAlignedObjectArray<btTransform> m_childTransforms;
+ btAlignedObjectArray<btCollisionShape*> m_childShapes;
+
+
+public:
+
+ btGImpactCompoundShape(bool children_has_transform = true)
+ {
+ m_primitive_manager.m_compoundShape = this;
+ m_box_set.setPrimitiveManager(&m_primitive_manager);
+ }
+
+ virtual ~btGImpactCompoundShape()
+ {
+ }
+
+
+ //! if true, then its children must get transforms.
+ virtual bool childrenHasTransform() const
+ {
+ if(m_childTransforms.size()==0) return false;
+ return true;
+ }
+
+
+ //! Obtains the primitive manager
+ virtual const btPrimitiveManagerBase * getPrimitiveManager() const
+ {
+ return &m_primitive_manager;
+ }
+
+ //! Obtains the compopund primitive manager
+ SIMD_FORCE_INLINE CompoundPrimitiveManager * getCompoundPrimitiveManager()
+ {
+ return &m_primitive_manager;
+ }
+
+ //! Gets the number of children
+ virtual int getNumChildShapes() const
+ {
+ return m_childShapes.size();
+ }
+
+
+ //! Use this method for adding children. Only Convex shapes are allowed.
+ void addChildShape(const btTransform& localTransform,btCollisionShape* shape)
+ {
+ btAssert(shape->isConvex());
+ m_childTransforms.push_back(localTransform);
+ m_childShapes.push_back(shape);
+ }
+
+ //! Use this method for adding children. Only Convex shapes are allowed.
+ void addChildShape(btCollisionShape* shape)
+ {
+ btAssert(shape->isConvex());
+ m_childShapes.push_back(shape);
+ }
+
+ //! Gets the children
+ virtual btCollisionShape* getChildShape(int index)
+ {
+ return m_childShapes[index];
+ }
+
+ //! Gets the children
+ virtual const btCollisionShape* getChildShape(int index) const
+ {
+ return m_childShapes[index];
+ }
+
+ //! Retrieves the bound from a child
+ /*!
+ */
+ virtual void getChildAabb(int child_index,const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
+ {
+
+ if(childrenHasTransform())
+ {
+ m_childShapes[child_index]->getAabb(t*m_childTransforms[child_index],aabbMin,aabbMax);
+ }
+ else
+ {
+ m_childShapes[child_index]->getAabb(t,aabbMin,aabbMax);
+ }
+ }
+
+
+ //! Gets the children transform
+ virtual btTransform getChildTransform(int index) const
+ {
+ btAssert(m_childTransforms.size() == m_childShapes.size());
+ return m_childTransforms[index];
+ }
+
+ //! Sets the children transform
+ /*!
+ \post You must call updateBound() for update the box set.
+ */
+ virtual void setChildTransform(int index, const btTransform & transform)
+ {
+ btAssert(m_childTransforms.size() == m_childShapes.size());
+ m_childTransforms[index] = transform;
+ postUpdate();
+ }
+
+ //! Determines if this shape has triangles
+ virtual bool needsRetrieveTriangles() const
+ {
+ return false;
+ }
+
+ //! Determines if this shape has tetrahedrons
+ virtual bool needsRetrieveTetrahedrons() const
+ {
+ return false;
+ }
+
+
+ virtual void getBulletTriangle(int prim_index,btTriangleShapeEx & triangle) const
+ {
+ btAssert(0);
+ }
+
+ virtual void getBulletTetrahedron(int prim_index,btTetrahedronShapeEx & tetrahedron) const
+ {
+ btAssert(0);
+ }
+
+
+ //! Calculates the exact inertia tensor for this shape
+ virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+
+ virtual const char* getName()const
+ {
+ return "GImpactCompound";
+ }
+
+ virtual eGIMPACT_SHAPE_TYPE getGImpactShapeType()
+ {
+ return CONST_GIMPACT_COMPOUND_SHAPE;
+ }
+
+};
+
+
+
+//! This class manages a sub part of a mesh supplied by the btStridingMeshInterface interface.
+/*!
+- Simply create this shape by passing the btStridingMeshInterface to the constructor btGImpactMeshShapePart, then you must call updateBound() after creating the mesh
+- When making operations with this shape, you must call <b>lock</b> before accessing to the trimesh primitives, and then call <b>unlock</b>
+- You can handle deformable meshes with this shape, by calling postUpdate() every time when changing the mesh vertices.
+
+*/
+class btGImpactMeshShapePart : public btGImpactShapeInterface
+{
+public:
+ //! Trimesh primitive manager
+ /*!
+ Manages the info from btStridingMeshInterface object and controls the Lock/Unlock mechanism
+ */
+ class TrimeshPrimitiveManager:public btPrimitiveManagerBase
+ {
+ public:
+ btScalar m_margin;
+ btStridingMeshInterface * m_meshInterface;
+ btVector3 m_scale;
+ int m_part;
+ int m_lock_count;
+ const unsigned char *vertexbase;
+ int numverts;
+ PHY_ScalarType type;
+ int stride;
+ const unsigned char *indexbase;
+ int indexstride;
+ int numfaces;
+ PHY_ScalarType indicestype;
+
+ TrimeshPrimitiveManager()
+ {
+ m_meshInterface = NULL;
+ m_part = 0;
+ m_margin = 0.01f;
+ m_scale = btVector3(1.f,1.f,1.f);
+ m_lock_count = 0;
+ vertexbase = 0;
+ numverts = 0;
+ stride = 0;
+ indexbase = 0;
+ indexstride = 0;
+ numfaces = 0;
+ }
+
+ TrimeshPrimitiveManager(const TrimeshPrimitiveManager & manager)
+ {
+ m_meshInterface = manager.m_meshInterface;
+ m_part = manager.m_part;
+ m_margin = manager.m_margin;
+ m_scale = manager.m_scale;
+ m_lock_count = 0;
+ vertexbase = 0;
+ numverts = 0;
+ stride = 0;
+ indexbase = 0;
+ indexstride = 0;
+ numfaces = 0;
+
+ }
+
+ TrimeshPrimitiveManager(
+ btStridingMeshInterface * meshInterface, int part)
+ {
+ m_meshInterface = meshInterface;
+ m_part = part;
+ m_scale = m_meshInterface->getScaling();
+ m_margin = 0.1f;
+ m_lock_count = 0;
+ vertexbase = 0;
+ numverts = 0;
+ stride = 0;
+ indexbase = 0;
+ indexstride = 0;
+ numfaces = 0;
+
+ }
+
+ virtual ~TrimeshPrimitiveManager() {}
+
+ void lock()
+ {
+ if(m_lock_count>0)
+ {
+ m_lock_count++;
+ return;
+ }
+ m_meshInterface->getLockedReadOnlyVertexIndexBase(
+ &vertexbase,numverts,
+ type, stride,&indexbase, indexstride, numfaces,indicestype,m_part);
+
+ m_lock_count = 1;
+ }
+
+ void unlock()
+ {
+ if(m_lock_count == 0) return;
+ if(m_lock_count>1)
+ {
+ --m_lock_count;
+ return;
+ }
+ m_meshInterface->unLockReadOnlyVertexBase(m_part);
+ vertexbase = NULL;
+ m_lock_count = 0;
+ }
+
+ virtual bool is_trimesh() const
+ {
+ return true;
+ }
+
+ virtual int get_primitive_count() const
+ {
+ return (int )numfaces;
+ }
+
+ SIMD_FORCE_INLINE int get_vertex_count() const
+ {
+ return (int )numverts;
+ }
+
+ SIMD_FORCE_INLINE void get_indices(int face_index,int &i0,int &i1,int &i2) const
+ {
+ if(indicestype == PHY_SHORT)
+ {
+ short * s_indices = (short *)(indexbase + face_index*indexstride);
+ i0 = s_indices[0];
+ i1 = s_indices[1];
+ i2 = s_indices[2];
+ }
+ else
+ {
+ int * i_indices = (int *)(indexbase + face_index*indexstride);
+ i0 = i_indices[0];
+ i1 = i_indices[1];
+ i2 = i_indices[2];
+ }
+ }
+
+ SIMD_FORCE_INLINE void get_vertex(int vertex_index, btVector3 & vertex) const
+ {
+ if(type == PHY_DOUBLE)
+ {
+ double * dvertices = (double *)(vertexbase + vertex_index*stride);
+ vertex[0] = btScalar(dvertices[0]*m_scale[0]);
+ vertex[1] = btScalar(dvertices[1]*m_scale[1]);
+ vertex[2] = btScalar(dvertices[2]*m_scale[2]);
+ }
+ else
+ {
+ float * svertices = (float *)(vertexbase + vertex_index*stride);
+ vertex[0] = svertices[0]*m_scale[0];
+ vertex[1] = svertices[1]*m_scale[1];
+ vertex[2] = svertices[2]*m_scale[2];
+ }
+ }
+
+ virtual void get_primitive_box(int prim_index ,btAABB & primbox) const
+ {
+ btPrimitiveTriangle triangle;
+ get_primitive_triangle(prim_index,triangle);
+ primbox.calc_from_triangle_margin(
+ triangle.m_vertices[0],
+ triangle.m_vertices[1],triangle.m_vertices[2],triangle.m_margin);
+ }
+
+ virtual void get_primitive_triangle(int prim_index,btPrimitiveTriangle & triangle) const
+ {
+ int indices[3];
+ get_indices(prim_index,indices[0],indices[1],indices[2]);
+ get_vertex(indices[0],triangle.m_vertices[0]);
+ get_vertex(indices[1],triangle.m_vertices[1]);
+ get_vertex(indices[2],triangle.m_vertices[2]);
+ triangle.m_margin = m_margin;
+ }
+
+ SIMD_FORCE_INLINE void get_bullet_triangle(int prim_index,btTriangleShapeEx & triangle) const
+ {
+ int indices[3];
+ get_indices(prim_index,indices[0],indices[1],indices[2]);
+ get_vertex(indices[0],triangle.m_vertices1[0]);
+ get_vertex(indices[1],triangle.m_vertices1[1]);
+ get_vertex(indices[2],triangle.m_vertices1[2]);
+ triangle.setMargin(m_margin);
+ }
+
+ };
+
+
+protected:
+ TrimeshPrimitiveManager m_primitive_manager;
+public:
+
+ btGImpactMeshShapePart()
+ {
+ m_box_set.setPrimitiveManager(&m_primitive_manager);
+ }
+
+
+ btGImpactMeshShapePart(btStridingMeshInterface * meshInterface, int part)
+ {
+ m_primitive_manager.m_meshInterface = meshInterface;
+ m_primitive_manager.m_part = part;
+ m_box_set.setPrimitiveManager(&m_primitive_manager);
+ }
+
+ virtual ~btGImpactMeshShapePart()
+ {
+ }
+
+ //! if true, then its children must get transforms.
+ virtual bool childrenHasTransform() const
+ {
+ return false;
+ }
+
+
+ //! call when reading child shapes
+ virtual void lockChildShapes() const
+ {
+ void * dummy = (void*)(m_box_set.getPrimitiveManager());
+ TrimeshPrimitiveManager * dummymanager = static_cast<TrimeshPrimitiveManager *>(dummy);
+ dummymanager->lock();
+ }
+
+ virtual void unlockChildShapes() const
+ {
+ void * dummy = (void*)(m_box_set.getPrimitiveManager());
+ TrimeshPrimitiveManager * dummymanager = static_cast<TrimeshPrimitiveManager *>(dummy);
+ dummymanager->unlock();
+ }
+
+ //! Gets the number of children
+ virtual int getNumChildShapes() const
+ {
+ return m_primitive_manager.get_primitive_count();
+ }
+
+
+ //! Gets the children
+ virtual btCollisionShape* getChildShape(int index)
+ {
+ btAssert(0);
+ return NULL;
+ }
+
+
+
+ //! Gets the child
+ virtual const btCollisionShape* getChildShape(int index) const
+ {
+ btAssert(0);
+ return NULL;
+ }
+
+ //! Gets the children transform
+ virtual btTransform getChildTransform(int index) const
+ {
+ btAssert(0);
+ return btTransform();
+ }
+
+ //! Sets the children transform
+ /*!
+ \post You must call updateBound() for update the box set.
+ */
+ virtual void setChildTransform(int index, const btTransform & transform)
+ {
+ btAssert(0);
+ }
+
+
+ //! Obtains the primitive manager
+ virtual const btPrimitiveManagerBase * getPrimitiveManager() const
+ {
+ return &m_primitive_manager;
+ }
+
+ SIMD_FORCE_INLINE TrimeshPrimitiveManager * getTrimeshPrimitiveManager()
+ {
+ return &m_primitive_manager;
+ }
+
+
+
+
+
+ virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+
+
+
+
+ virtual const char* getName()const
+ {
+ return "GImpactMeshShapePart";
+ }
+
+ virtual eGIMPACT_SHAPE_TYPE getGImpactShapeType()
+ {
+ return CONST_GIMPACT_TRIMESH_SHAPE_PART;
+ }
+
+ //! Determines if this shape has triangles
+ virtual bool needsRetrieveTriangles() const
+ {
+ return true;
+ }
+
+ //! Determines if this shape has tetrahedrons
+ virtual bool needsRetrieveTetrahedrons() const
+ {
+ return false;
+ }
+
+ virtual void getBulletTriangle(int prim_index,btTriangleShapeEx & triangle) const
+ {
+ m_primitive_manager.get_bullet_triangle(prim_index,triangle);
+ }
+
+ virtual void getBulletTetrahedron(int prim_index,btTetrahedronShapeEx & tetrahedron) const
+ {
+ btAssert(0);
+ }
+
+
+
+ SIMD_FORCE_INLINE int getVertexCount() const
+ {
+ return m_primitive_manager.get_vertex_count();
+ }
+
+ SIMD_FORCE_INLINE void getVertex(int vertex_index, btVector3 & vertex) const
+ {
+ m_primitive_manager.get_vertex(vertex_index,vertex);
+ }
+
+ SIMD_FORCE_INLINE void setMargin(btScalar margin)
+ {
+ m_primitive_manager.m_margin = margin;
+ postUpdate();
+ }
+
+ SIMD_FORCE_INLINE btScalar getMargin() const
+ {
+ return m_primitive_manager.m_margin;
+ }
+
+ virtual void setLocalScaling(const btVector3& scaling)
+ {
+ m_primitive_manager.m_scale = scaling;
+ postUpdate();
+ }
+
+ virtual const btVector3& getLocalScaling() const
+ {
+ return m_primitive_manager.m_scale;
+ }
+
+ SIMD_FORCE_INLINE int getPart() const
+ {
+ return (int)m_primitive_manager.m_part;
+ }
+
+ virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const;
+};
+
+
+//! This class manages a mesh supplied by the btStridingMeshInterface interface.
+/*!
+Set of btGImpactMeshShapePart parts
+- Simply create this shape by passing the btStridingMeshInterface to the constructor btGImpactMeshShape, then you must call updateBound() after creating the mesh
+
+- You can handle deformable meshes with this shape, by calling postUpdate() every time when changing the mesh vertices.
+
+*/
+class btGImpactMeshShape : public btGImpactShapeInterface
+{
+ btStridingMeshInterface* m_meshInterface;
+
+protected:
+ btAlignedObjectArray<btGImpactMeshShapePart*> m_mesh_parts;
+ void buildMeshParts(btStridingMeshInterface * meshInterface)
+ {
+ for (int i=0;i<meshInterface->getNumSubParts() ;++i )
+ {
+ btGImpactMeshShapePart * newpart = new btGImpactMeshShapePart(meshInterface,i);
+ m_mesh_parts.push_back(newpart);
+ }
+ }
+
+ //! use this function for perfofm refit in bounding boxes
+ virtual void calcLocalAABB()
+ {
+ m_localAABB.invalidate();
+ int i = m_mesh_parts.size();
+ while(i--)
+ {
+ m_mesh_parts[i]->updateBound();
+ m_localAABB.merge(m_mesh_parts[i]->getLocalBox());
+ }
+ }
+
+public:
+ btGImpactMeshShape(btStridingMeshInterface * meshInterface)
+ {
+ m_meshInterface = meshInterface;
+ buildMeshParts(meshInterface);
+ }
+
+ virtual ~btGImpactMeshShape()
+ {
+ int i = m_mesh_parts.size();
+ while(i--)
+ {
+ btGImpactMeshShapePart * part = m_mesh_parts[i];
+ delete part;
+ }
+ m_mesh_parts.clear();
+ }
+
+
+ btStridingMeshInterface* getMeshInterface()
+ {
+ return m_meshInterface;
+ }
+
+ const btStridingMeshInterface* getMeshInterface() const
+ {
+ return m_meshInterface;
+ }
+
+ int getMeshPartCount() const
+ {
+ return m_mesh_parts.size();
+ }
+
+ btGImpactMeshShapePart * getMeshPart(int index)
+ {
+ return m_mesh_parts[index];
+ }
+
+
+
+ const btGImpactMeshShapePart * getMeshPart(int index) const
+ {
+ return m_mesh_parts[index];
+ }
+
+
+ virtual void setLocalScaling(const btVector3& scaling)
+ {
+ localScaling = scaling;
+
+ int i = m_mesh_parts.size();
+ while(i--)
+ {
+ btGImpactMeshShapePart * part = m_mesh_parts[i];
+ part->setLocalScaling(scaling);
+ }
+
+ m_needs_update = true;
+ }
+
+ virtual void setMargin(btScalar margin)
+ {
+ m_collisionMargin = margin;
+
+ int i = m_mesh_parts.size();
+ while(i--)
+ {
+ btGImpactMeshShapePart * part = m_mesh_parts[i];
+ part->setMargin(margin);
+ }
+
+ m_needs_update = true;
+ }
+
+ //! Tells to this object that is needed to refit all the meshes
+ virtual void postUpdate()
+ {
+ int i = m_mesh_parts.size();
+ while(i--)
+ {
+ btGImpactMeshShapePart * part = m_mesh_parts[i];
+ part->postUpdate();
+ }
+
+ m_needs_update = true;
+ }
+
+ virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+
+
+ //! Obtains the primitive manager
+ virtual const btPrimitiveManagerBase * getPrimitiveManager() const
+ {
+ btAssert(0);
+ return NULL;
+ }
+
+
+ //! Gets the number of children
+ virtual int getNumChildShapes() const
+ {
+ btAssert(0);
+ return 0;
+ }
+
+
+ //! if true, then its children must get transforms.
+ virtual bool childrenHasTransform() const
+ {
+ btAssert(0);
+ return false;
+ }
+
+ //! Determines if this shape has triangles
+ virtual bool needsRetrieveTriangles() const
+ {
+ btAssert(0);
+ return false;
+ }
+
+ //! Determines if this shape has tetrahedrons
+ virtual bool needsRetrieveTetrahedrons() const
+ {
+ btAssert(0);
+ return false;
+ }
+
+ virtual void getBulletTriangle(int prim_index,btTriangleShapeEx & triangle) const
+ {
+ btAssert(0);
+ }
+
+ virtual void getBulletTetrahedron(int prim_index,btTetrahedronShapeEx & tetrahedron) const
+ {
+ btAssert(0);
+ }
+
+ //! call when reading child shapes
+ virtual void lockChildShapes() const
+ {
+ btAssert(0);
+ }
+
+ virtual void unlockChildShapes() const
+ {
+ btAssert(0);
+ }
+
+
+
+
+ //! Retrieves the bound from a child
+ /*!
+ */
+ virtual void getChildAabb(int child_index,const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
+ {
+ btAssert(0);
+ }
+
+ //! Gets the children
+ virtual btCollisionShape* getChildShape(int index)
+ {
+ btAssert(0);
+ return NULL;
+ }
+
+
+ //! Gets the child
+ virtual const btCollisionShape* getChildShape(int index) const
+ {
+ btAssert(0);
+ return NULL;
+ }
+
+ //! Gets the children transform
+ virtual btTransform getChildTransform(int index) const
+ {
+ btAssert(0);
+ return btTransform();
+ }
+
+ //! Sets the children transform
+ /*!
+ \post You must call updateBound() for update the box set.
+ */
+ virtual void setChildTransform(int index, const btTransform & transform)
+ {
+ btAssert(0);
+ }
+
+
+ virtual eGIMPACT_SHAPE_TYPE getGImpactShapeType()
+ {
+ return CONST_GIMPACT_TRIMESH_SHAPE;
+ }
+
+
+ virtual const char* getName()const
+ {
+ return "GImpactMesh";
+ }
+
+ virtual void rayTest(const btVector3& rayFrom, const btVector3& rayTo, btCollisionWorld::RayResultCallback& resultCallback) const;
+
+ //! Function for retrieve triangles.
+ /*!
+ It gives the triangles in local space
+ */
+ virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const;
+};
+
+
+#endif //GIMPACT_MESH_SHAPE_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btGenericPoolAllocator.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btGenericPoolAllocator.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btGenericPoolAllocator.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btGenericPoolAllocator.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,163 @@
+/*! \file btGenericPoolAllocator.h
+\author Francisco Len Nßjera. email projectileman at yahoo.com
+
+General purpose allocator class
+*/
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_GENERIC_POOL_ALLOCATOR_H
+#define BT_GENERIC_POOL_ALLOCATOR_H
+
+#include <limits.h>
+#include <stdio.h>
+#include <string.h>
+#include "LinearMath/btAlignedAllocator.h"
+
+#define BT_UINT_MAX UINT_MAX
+#define BT_DEFAULT_MAX_POOLS 16
+
+
+//! Generic Pool class
+class btGenericMemoryPool
+{
+public:
+ unsigned char * m_pool; //[m_element_size*m_max_element_count];
+ size_t * m_free_nodes; //[m_max_element_count];//! free nodes
+ size_t * m_allocated_sizes;//[m_max_element_count];//! Number of elements allocated per node
+ size_t m_allocated_count;
+ size_t m_free_nodes_count;
+protected:
+ size_t m_element_size;
+ size_t m_max_element_count;
+
+ size_t allocate_from_free_nodes(size_t num_elements);
+ size_t allocate_from_pool(size_t num_elements);
+
+public:
+
+ void init_pool(size_t element_size, size_t element_count);
+
+ void end_pool();
+
+
+ btGenericMemoryPool(size_t element_size, size_t element_count)
+ {
+ init_pool(element_size, element_count);
+ }
+
+ ~btGenericMemoryPool()
+ {
+ end_pool();
+ }
+
+
+ inline size_t get_pool_capacity()
+ {
+ return m_element_size*m_max_element_count;
+ }
+
+ inline size_t gem_element_size()
+ {
+ return m_element_size;
+ }
+
+ inline size_t get_max_element_count()
+ {
+ return m_max_element_count;
+ }
+
+ inline size_t get_allocated_count()
+ {
+ return m_allocated_count;
+ }
+
+ inline size_t get_free_positions_count()
+ {
+ return m_free_nodes_count;
+ }
+
+ inline void * get_element_data(size_t element_index)
+ {
+ return &m_pool[element_index*m_element_size];
+ }
+
+ //! Allocates memory in pool
+ /*!
+ \param size_bytes size in bytes of the buffer
+ */
+ void * allocate(size_t size_bytes);
+
+ bool freeMemory(void * pointer);
+};
+
+
+
+
+//! Generic Allocator with pools
+/*!
+General purpose Allocator which can create Memory Pools dynamiacally as needed.
+*/
+class btGenericPoolAllocator
+{
+protected:
+ size_t m_pool_element_size;
+ size_t m_pool_element_count;
+public:
+ btGenericMemoryPool * m_pools[BT_DEFAULT_MAX_POOLS];
+ size_t m_pool_count;
+
+
+ inline size_t get_pool_capacity()
+ {
+ return m_pool_element_size*m_pool_element_count;
+ }
+
+
+protected:
+ // creates a pool
+ btGenericMemoryPool * push_new_pool();
+
+ void * failback_alloc(size_t size_bytes);
+
+ bool failback_free(void * pointer);
+public:
+
+ btGenericPoolAllocator(size_t pool_element_size, size_t pool_element_count)
+ {
+ m_pool_count = 0;
+ m_pool_element_size = pool_element_size;
+ m_pool_element_count = pool_element_count;
+ }
+
+ virtual ~btGenericPoolAllocator();
+
+ //! Allocates memory in pool
+ /*!
+ \param size_bytes size in bytes of the buffer
+ */
+ void * allocate(size_t size_bytes);
+
+ bool freeMemory(void * pointer);
+};
+
+
+
+void * btPoolAlloc(size_t size);
+void * btPoolRealloc(void *ptr, size_t oldsize, size_t newsize);
+void btPoolFree(void *ptr);
+
+
+#endif
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btGeometryOperations.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btGeometryOperations.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btGeometryOperations.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btGeometryOperations.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,212 @@
+#ifndef BT_BASIC_GEOMETRY_OPERATIONS_H_INCLUDED
+#define BT_BASIC_GEOMETRY_OPERATIONS_H_INCLUDED
+
+/*! \file btGeometryOperations.h
+*\author Francisco Len Nßjera
+
+*/
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman at yahoo.com
+
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btBoxCollision.h"
+
+
+
+
+
+#define PLANEDIREPSILON 0.0000001f
+#define PARALELENORMALS 0.000001f
+
+
+#define BT_CLAMP(number,minval,maxval) (number<minval?minval:(number>maxval?maxval:number))
+
+/// Calc a plane from a triangle edge an a normal. plane is a vec4f
+SIMD_FORCE_INLINE void bt_edge_plane(const btVector3 & e1,const btVector3 & e2, const btVector3 & normal,btVector4 & plane)
+{
+ btVector3 planenormal = (e2-e1).cross(normal);
+ planenormal.normalize();
+ plane.setValue(planenormal[0],planenormal[1],planenormal[2],e2.dot(planenormal));
+}
+
+
+
+//***************** SEGMENT and LINE FUNCTIONS **********************************///
+
+/*! Finds the closest point(cp) to (v) on a segment (e1,e2)
+ */
+SIMD_FORCE_INLINE void bt_closest_point_on_segment(
+ btVector3 & cp, const btVector3 & v,
+ const btVector3 &e1,const btVector3 &e2)
+{
+ btVector3 n = e2-e1;
+ cp = v - e1;
+ btScalar _scalar = cp.dot(n)/n.dot(n);
+ if(_scalar <0.0f)
+ {
+ cp = e1;
+ }
+ else if(_scalar >1.0f)
+ {
+ cp = e2;
+ }
+ else
+ {
+ cp = _scalar*n + e1;
+ }
+}
+
+
+//! line plane collision
+/*!
+*\return
+ -0 if the ray never intersects
+ -1 if the ray collides in front
+ -2 if the ray collides in back
+*/
+
+SIMD_FORCE_INLINE int bt_line_plane_collision(
+ const btVector4 & plane,
+ const btVector3 & vDir,
+ const btVector3 & vPoint,
+ btVector3 & pout,
+ btScalar &tparam,
+ btScalar tmin, btScalar tmax)
+{
+
+ btScalar _dotdir = vDir.dot(plane);
+
+ if(btFabs(_dotdir)<PLANEDIREPSILON)
+ {
+ tparam = tmax;
+ return 0;
+ }
+
+ btScalar _dis = bt_distance_point_plane(plane,vPoint);
+ char returnvalue = _dis<0.0f? 2:1;
+ tparam = -_dis/_dotdir;
+
+ if(tparam<tmin)
+ {
+ returnvalue = 0;
+ tparam = tmin;
+ }
+ else if(tparam>tmax)
+ {
+ returnvalue = 0;
+ tparam = tmax;
+ }
+ pout = tparam*vDir + vPoint;
+ return returnvalue;
+}
+
+
+//! Find closest points on segments
+SIMD_FORCE_INLINE void bt_segment_collision(
+ const btVector3 & vA1,
+ const btVector3 & vA2,
+ const btVector3 & vB1,
+ const btVector3 & vB2,
+ btVector3 & vPointA,
+ btVector3 & vPointB)
+{
+ btVector3 AD = vA2 - vA1;
+ btVector3 BD = vB2 - vB1;
+ btVector3 N = AD.cross(BD);
+ btScalar tp = N.length2();
+
+ btVector4 _M;//plane
+
+ if(tp<SIMD_EPSILON)//ARE PARALELE
+ {
+ //project B over A
+ bool invert_b_order = false;
+ _M[0] = vB1.dot(AD);
+ _M[1] = vB2.dot(AD);
+
+ if(_M[0]>_M[1])
+ {
+ invert_b_order = true;
+ BT_SWAP_NUMBERS(_M[0],_M[1]);
+ }
+ _M[2] = vA1.dot(AD);
+ _M[3] = vA2.dot(AD);
+ //mid points
+ N[0] = (_M[0]+_M[1])*0.5f;
+ N[1] = (_M[2]+_M[3])*0.5f;
+
+ if(N[0]<N[1])
+ {
+ if(_M[1]<_M[2])
+ {
+ vPointB = invert_b_order?vB1:vB2;
+ vPointA = vA1;
+ }
+ else if(_M[1]<_M[3])
+ {
+ vPointB = invert_b_order?vB1:vB2;
+ bt_closest_point_on_segment(vPointA,vPointB,vA1,vA2);
+ }
+ else
+ {
+ vPointA = vA2;
+ bt_closest_point_on_segment(vPointB,vPointA,vB1,vB2);
+ }
+ }
+ else
+ {
+ if(_M[3]<_M[0])
+ {
+ vPointB = invert_b_order?vB2:vB1;
+ vPointA = vA2;
+ }
+ else if(_M[3]<_M[1])
+ {
+ vPointA = vA2;
+ bt_closest_point_on_segment(vPointB,vPointA,vB1,vB2);
+ }
+ else
+ {
+ vPointB = invert_b_order?vB1:vB2;
+ bt_closest_point_on_segment(vPointA,vPointB,vA1,vA2);
+ }
+ }
+ return;
+ }
+
+ N = N.cross(BD);
+ _M.setValue(N[0],N[1],N[2],vB1.dot(N));
+
+ // get point A as the plane collision point
+ bt_line_plane_collision(_M,AD,vA1,vPointA,tp,btScalar(0), btScalar(1));
+
+ /*Closest point on segment*/
+ vPointB = vPointA - vB1;
+ tp = vPointB.dot(BD);
+ tp/= BD.dot(BD);
+ tp = BT_CLAMP(tp,0.0f,1.0f);
+
+ vPointB = tp*BD + vB1;
+}
+
+
+
+
+
+#endif // GIM_VECTOR_H_INCLUDED
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btQuantization.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btQuantization.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btQuantization.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btQuantization.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,88 @@
+#ifndef BT_QUANTIZATION_H_INCLUDED
+#define BT_QUANTIZATION_H_INCLUDED
+
+/*! \file btQuantization.h
+*\author Francisco Len Nßjera
+
+*/
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman at yahoo.com
+
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "LinearMath/btTransform.h"
+
+
+
+
+
+
+SIMD_FORCE_INLINE void bt_calc_quantization_parameters(
+ btVector3 & outMinBound,
+ btVector3 & outMaxBound,
+ btVector3 & bvhQuantization,
+ const btVector3& srcMinBound,const btVector3& srcMaxBound,
+ btScalar quantizationMargin)
+{
+ //enlarge the AABB to avoid division by zero when initializing the quantization values
+ btVector3 clampValue(quantizationMargin,quantizationMargin,quantizationMargin);
+ outMinBound = srcMinBound - clampValue;
+ outMaxBound = srcMaxBound + clampValue;
+ btVector3 aabbSize = outMaxBound - outMinBound;
+ bvhQuantization = btVector3(btScalar(65535.0),
+ btScalar(65535.0),
+ btScalar(65535.0)) / aabbSize;
+}
+
+
+SIMD_FORCE_INLINE void bt_quantize_clamp(
+ unsigned short* out,
+ const btVector3& point,
+ const btVector3 & min_bound,
+ const btVector3 & max_bound,
+ const btVector3 & bvhQuantization)
+{
+
+ btVector3 clampedPoint(point);
+ clampedPoint.setMax(min_bound);
+ clampedPoint.setMin(max_bound);
+
+ btVector3 v = (clampedPoint - min_bound) * bvhQuantization;
+ out[0] = (unsigned short)(v.getX()+0.5f);
+ out[1] = (unsigned short)(v.getY()+0.5f);
+ out[2] = (unsigned short)(v.getZ()+0.5f);
+}
+
+
+SIMD_FORCE_INLINE btVector3 bt_unquantize(
+ const unsigned short* vecIn,
+ const btVector3 & offset,
+ const btVector3 & bvhQuantization)
+{
+ btVector3 vecOut;
+ vecOut.setValue(
+ (btScalar)(vecIn[0]) / (bvhQuantization.getX()),
+ (btScalar)(vecIn[1]) / (bvhQuantization.getY()),
+ (btScalar)(vecIn[2]) / (bvhQuantization.getZ()));
+ vecOut += offset;
+ return vecOut;
+}
+
+
+
+#endif // GIM_VECTOR_H_INCLUDED
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btTriangleShapeEx.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btTriangleShapeEx.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btTriangleShapeEx.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/btTriangleShapeEx.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,180 @@
+/*! \file btGImpactShape.h
+\author Francisco Len Nßjera
+*/
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman at yahoo.com
+
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef TRIANGLE_SHAPE_EX_H
+#define TRIANGLE_SHAPE_EX_H
+
+#include "BulletCollision/CollisionShapes/btCollisionShape.h"
+#include "BulletCollision/CollisionShapes/btTriangleShape.h"
+#include "btBoxCollision.h"
+#include "btClipPolygon.h"
+#include "btGeometryOperations.h"
+
+
+#define MAX_TRI_CLIPPING 16
+
+//! Structure for collision
+struct GIM_TRIANGLE_CONTACT
+{
+ btScalar m_penetration_depth;
+ int m_point_count;
+ btVector4 m_separating_normal;
+ btVector3 m_points[MAX_TRI_CLIPPING];
+
+ SIMD_FORCE_INLINE void copy_from(const GIM_TRIANGLE_CONTACT& other)
+ {
+ m_penetration_depth = other.m_penetration_depth;
+ m_separating_normal = other.m_separating_normal;
+ m_point_count = other.m_point_count;
+ int i = m_point_count;
+ while(i--)
+ {
+ m_points[i] = other.m_points[i];
+ }
+ }
+
+ GIM_TRIANGLE_CONTACT()
+ {
+ }
+
+ GIM_TRIANGLE_CONTACT(const GIM_TRIANGLE_CONTACT& other)
+ {
+ copy_from(other);
+ }
+
+ //! classify points that are closer
+ void merge_points(const btVector4 & plane,
+ btScalar margin, const btVector3 * points, int point_count);
+
+};
+
+
+
+class btPrimitiveTriangle
+{
+public:
+ btVector3 m_vertices[3];
+ btVector4 m_plane;
+ btScalar m_margin;
+ btScalar m_dummy;
+ btPrimitiveTriangle():m_margin(0.01f)
+ {
+
+ }
+
+
+ SIMD_FORCE_INLINE void buildTriPlane()
+ {
+ btVector3 normal = (m_vertices[1]-m_vertices[0]).cross(m_vertices[2]-m_vertices[0]);
+ normal.normalize();
+ m_plane.setValue(normal[0],normal[1],normal[2],m_vertices[0].dot(normal));
+ }
+
+ //! Test if triangles could collide
+ bool overlap_test_conservative(const btPrimitiveTriangle& other);
+
+ //! Calcs the plane which is paralele to the edge and perpendicular to the triangle plane
+ /*!
+ \pre this triangle must have its plane calculated.
+ */
+ SIMD_FORCE_INLINE void get_edge_plane(int edge_index, btVector4 &plane) const
+ {
+ const btVector3 & e0 = m_vertices[edge_index];
+ const btVector3 & e1 = m_vertices[(edge_index+1)%3];
+ bt_edge_plane(e0,e1,m_plane,plane);
+ }
+
+ void applyTransform(const btTransform& t)
+ {
+ m_vertices[0] = t(m_vertices[0]);
+ m_vertices[1] = t(m_vertices[1]);
+ m_vertices[2] = t(m_vertices[2]);
+ }
+
+ //! Clips the triangle against this
+ /*!
+ \pre clipped_points must have MAX_TRI_CLIPPING size, and this triangle must have its plane calculated.
+ \return the number of clipped points
+ */
+ int clip_triangle(btPrimitiveTriangle & other, btVector3 * clipped_points );
+
+ //! Find collision using the clipping method
+ /*!
+ \pre this triangle and other must have their triangles calculated
+ */
+ bool find_triangle_collision_clip_method(btPrimitiveTriangle & other, GIM_TRIANGLE_CONTACT & contacts);
+};
+
+
+
+//! Helper class for colliding Bullet Triangle Shapes
+/*!
+This class implements a better getAabb method than the previous btTriangleShape class
+*/
+class btTriangleShapeEx: public btTriangleShape
+{
+public:
+
+ btTriangleShapeEx():btTriangleShape(btVector3(0,0,0),btVector3(0,0,0),btVector3(0,0,0))
+ {
+ }
+
+ btTriangleShapeEx(const btVector3& p0,const btVector3& p1,const btVector3& p2): btTriangleShape(p0,p1,p2)
+ {
+ }
+
+ btTriangleShapeEx(const btTriangleShapeEx & other): btTriangleShape(other.m_vertices1[0],other.m_vertices1[1],other.m_vertices1[2])
+ {
+ }
+
+ virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax)const
+ {
+ btVector3 tv0 = t(m_vertices1[0]);
+ btVector3 tv1 = t(m_vertices1[1]);
+ btVector3 tv2 = t(m_vertices1[2]);
+
+ btAABB trianglebox(tv0,tv1,tv2,m_collisionMargin);
+ aabbMin = trianglebox.m_min;
+ aabbMax = trianglebox.m_max;
+ }
+
+ void applyTransform(const btTransform& t)
+ {
+ m_vertices1[0] = t(m_vertices1[0]);
+ m_vertices1[1] = t(m_vertices1[1]);
+ m_vertices1[2] = t(m_vertices1[2]);
+ }
+
+ SIMD_FORCE_INLINE void buildTriPlane(btVector4 & plane) const
+ {
+ btVector3 normal = (m_vertices1[1]-m_vertices1[0]).cross(m_vertices1[2]-m_vertices1[0]);
+ normal.normalize();
+ plane.setValue(normal[0],normal[1],normal[2],m_vertices1[0].dot(normal));
+ }
+
+ bool overlap_test_conservative(const btTriangleShapeEx& other);
+};
+
+
+#endif //TRIANGLE_MESH_SHAPE_H
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_array.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_array.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_array.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_array.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,326 @@
+#ifndef GIM_ARRAY_H_INCLUDED
+#define GIM_ARRAY_H_INCLUDED
+/*! \file gim_array.h
+\author Francisco Len Nßjera
+*/
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman at yahoo.com
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of EITHER:
+ (1) The GNU Lesser General Public License as published by the Free
+ Software Foundation; either version 2.1 of the License, or (at
+ your option) any later version. The text of the GNU Lesser
+ General Public License is included with this library in the
+ file GIMPACT-LICENSE-LGPL.TXT.
+ (2) The BSD-style license that is included with this library in
+ the file GIMPACT-LICENSE-BSD.TXT.
+ (3) The zlib/libpng license that is included with this library in
+ the file GIMPACT-LICENSE-ZLIB.TXT.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
+ GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+
+#include "gim_memory.h"
+
+
+#define GIM_ARRAY_GROW_INCREMENT 2
+#define GIM_ARRAY_GROW_FACTOR 2
+
+//! Very simple array container with fast access and simd memory
+template<typename T>
+class gim_array
+{
+public:
+//! properties
+//!@{
+ T *m_data;
+ GUINT m_size;
+ GUINT m_allocated_size;
+//!@}
+//! protected operations
+//!@{
+
+ inline void destroyData()
+ {
+ m_allocated_size = 0;
+ if(m_data==NULL) return;
+ gim_free(m_data);
+ m_data = NULL;
+ }
+
+ inline bool resizeData(GUINT newsize)
+ {
+ if(newsize==0)
+ {
+ destroyData();
+ return true;
+ }
+
+ if(m_size>0)
+ {
+ m_data = (T*)gim_realloc(m_data,m_size*sizeof(T),newsize*sizeof(T));
+ }
+ else
+ {
+ m_data = (T*)gim_alloc(newsize*sizeof(T));
+ }
+ m_allocated_size = newsize;
+ return true;
+ }
+
+ inline bool growingCheck()
+ {
+ if(m_allocated_size<=m_size)
+ {
+ GUINT requestsize = m_size;
+ m_size = m_allocated_size;
+ if(resizeData((requestsize+GIM_ARRAY_GROW_INCREMENT)*GIM_ARRAY_GROW_FACTOR)==false) return false;
+ }
+ return true;
+ }
+
+//!@}
+//! public operations
+//!@{
+ inline bool reserve(GUINT size)
+ {
+ if(m_allocated_size>=size) return false;
+ return resizeData(size);
+ }
+
+ inline void clear_range(GUINT start_range)
+ {
+ while(m_size>start_range)
+ {
+ m_data[--m_size].~T();
+ }
+ }
+
+ inline void clear()
+ {
+ if(m_size==0)return;
+ clear_range(0);
+ }
+
+ inline void clear_memory()
+ {
+ clear();
+ destroyData();
+ }
+
+ gim_array()
+ {
+ m_data = 0;
+ m_size = 0;
+ m_allocated_size = 0;
+ }
+
+ gim_array(GUINT reservesize)
+ {
+ m_data = 0;
+ m_size = 0;
+
+ m_allocated_size = 0;
+ reserve(reservesize);
+ }
+
+ ~gim_array()
+ {
+ clear_memory();
+ }
+
+ inline GUINT size() const
+ {
+ return m_size;
+ }
+
+ inline GUINT max_size() const
+ {
+ return m_allocated_size;
+ }
+
+ inline T & operator[](size_t i)
+ {
+ return m_data[i];
+ }
+ inline const T & operator[](size_t i) const
+ {
+ return m_data[i];
+ }
+
+ inline T * pointer(){ return m_data;}
+ inline const T * pointer() const
+ { return m_data;}
+
+
+ inline T * get_pointer_at(GUINT i)
+ {
+ return m_data + i;
+ }
+
+ inline const T * get_pointer_at(GUINT i) const
+ {
+ return m_data + i;
+ }
+
+ inline T & at(GUINT i)
+ {
+ return m_data[i];
+ }
+
+ inline const T & at(GUINT i) const
+ {
+ return m_data[i];
+ }
+
+ inline T & front()
+ {
+ return *m_data;
+ }
+
+ inline const T & front() const
+ {
+ return *m_data;
+ }
+
+ inline T & back()
+ {
+ return m_data[m_size-1];
+ }
+
+ inline const T & back() const
+ {
+ return m_data[m_size-1];
+ }
+
+
+ inline void swap(GUINT i, GUINT j)
+ {
+ gim_swap_elements(m_data,i,j);
+ }
+
+ inline void push_back(const T & obj)
+ {
+ this->growingCheck();
+ m_data[m_size] = obj;
+ m_size++;
+ }
+
+ //!Simply increase the m_size, doesn't call the new element constructor
+ inline void push_back_mem()
+ {
+ this->growingCheck();
+ m_size++;
+ }
+
+ inline void push_back_memcpy(const T & obj)
+ {
+ this->growingCheck();
+ irr_simd_memcpy(&m_data[m_size],&obj,sizeof(T));
+ m_size++;
+ }
+
+ inline void pop_back()
+ {
+ m_size--;
+ m_data[m_size].~T();
+ }
+
+ //!Simply decrease the m_size, doesn't call the deleted element destructor
+ inline void pop_back_mem()
+ {
+ m_size--;
+ }
+
+ //! fast erase
+ inline void erase(GUINT index)
+ {
+ if(index<m_size-1)
+ {
+ swap(index,m_size-1);
+ }
+ pop_back();
+ }
+
+ inline void erase_sorted_mem(GUINT index)
+ {
+ m_size--;
+ for(GUINT i = index;i<m_size;i++)
+ {
+ gim_simd_memcpy(m_data+i,m_data+i+1,sizeof(T));
+ }
+ }
+
+ inline void erase_sorted(GUINT index)
+ {
+ m_data[index].~T();
+ erase_sorted_mem(index);
+ }
+
+ inline void insert_mem(GUINT index)
+ {
+ this->growingCheck();
+ for(GUINT i = m_size;i>index;i--)
+ {
+ gim_simd_memcpy(m_data+i,m_data+i-1,sizeof(T));
+ }
+ m_size++;
+ }
+
+ inline void insert(const T & obj,GUINT index)
+ {
+ insert_mem(index);
+ m_data[index] = obj;
+ }
+
+ inline void resize(GUINT size, bool call_constructor = true)
+ {
+
+ if(size>m_size)
+ {
+ reserve(size);
+ if(call_constructor)
+ {
+ T obj;
+ while(m_size<size)
+ {
+ m_data[m_size] = obj;
+ m_size++;
+ }
+ }
+ else
+ {
+ m_size = size;
+ }
+ }
+ else if(size<m_size)
+ {
+ if(call_constructor) clear_range(size);
+ m_size = size;
+ }
+ }
+
+ inline void refit()
+ {
+ resizeData(m_size);
+ }
+
+};
+
+
+
+
+
+#endif // GIM_CONTAINERS_H_INCLUDED
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_basic_geometry_operations.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_basic_geometry_operations.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_basic_geometry_operations.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_basic_geometry_operations.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,543 @@
+#ifndef GIM_BASIC_GEOMETRY_OPERATIONS_H_INCLUDED
+#define GIM_BASIC_GEOMETRY_OPERATIONS_H_INCLUDED
+
+/*! \file gim_basic_geometry_operations.h
+*\author Francisco Len Nßjera
+type independant geometry routines
+
+*/
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman at yahoo.com
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of EITHER:
+ (1) The GNU Lesser General Public License as published by the Free
+ Software Foundation; either version 2.1 of the License, or (at
+ your option) any later version. The text of the GNU Lesser
+ General Public License is included with this library in the
+ file GIMPACT-LICENSE-LGPL.TXT.
+ (2) The BSD-style license that is included with this library in
+ the file GIMPACT-LICENSE-BSD.TXT.
+ (3) The zlib/libpng license that is included with this library in
+ the file GIMPACT-LICENSE-ZLIB.TXT.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
+ GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+
+
+#include "gim_linear_math.h"
+
+
+
+
+
+#define PLANEDIREPSILON 0.0000001f
+#define PARALELENORMALS 0.000001f
+
+
+#define TRIANGLE_NORMAL(v1,v2,v3,n)\
+{\
+ vec3f _dif1,_dif2;\
+ VEC_DIFF(_dif1,v2,v1);\
+ VEC_DIFF(_dif2,v3,v1);\
+ VEC_CROSS(n,_dif1,_dif2);\
+ VEC_NORMALIZE(n);\
+}\
+
+#define TRIANGLE_NORMAL_FAST(v1,v2,v3,n){\
+ vec3f _dif1,_dif2; \
+ VEC_DIFF(_dif1,v2,v1); \
+ VEC_DIFF(_dif2,v3,v1); \
+ VEC_CROSS(n,_dif1,_dif2); \
+}\
+
+/// plane is a vec4f
+#define TRIANGLE_PLANE(v1,v2,v3,plane) {\
+ TRIANGLE_NORMAL(v1,v2,v3,plane);\
+ plane[3] = VEC_DOT(v1,plane);\
+}\
+
+/// plane is a vec4f
+#define TRIANGLE_PLANE_FAST(v1,v2,v3,plane) {\
+ TRIANGLE_NORMAL_FAST(v1,v2,v3,plane);\
+ plane[3] = VEC_DOT(v1,plane);\
+}\
+
+/// Calc a plane from an edge an a normal. plane is a vec4f
+#define EDGE_PLANE(e1,e2,n,plane) {\
+ vec3f _dif; \
+ VEC_DIFF(_dif,e2,e1); \
+ VEC_CROSS(plane,_dif,n); \
+ VEC_NORMALIZE(plane); \
+ plane[3] = VEC_DOT(e1,plane);\
+}\
+
+#define DISTANCE_PLANE_POINT(plane,point) (VEC_DOT(plane,point) - plane[3])
+
+#define PROJECT_POINT_PLANE(point,plane,projected) {\
+ GREAL _dis;\
+ _dis = DISTANCE_PLANE_POINT(plane,point);\
+ VEC_SCALE(projected,-_dis,plane);\
+ VEC_SUM(projected,projected,point); \
+}\
+
+//! Verifies if a point is in the plane hull
+template<typename CLASS_POINT,typename CLASS_PLANE>
+SIMD_FORCE_INLINE bool POINT_IN_HULL(
+ const CLASS_POINT& point,const CLASS_PLANE * planes,GUINT plane_count)
+{
+ GREAL _dis;
+ for (GUINT _i = 0;_i< plane_count;++_i)
+ {
+ _dis = DISTANCE_PLANE_POINT(planes[_i],point);
+ if(_dis>0.0f) return false;
+ }
+ return true;
+}
+
+template<typename CLASS_POINT,typename CLASS_PLANE>
+SIMD_FORCE_INLINE void PLANE_CLIP_SEGMENT(
+ const CLASS_POINT& s1,
+ const CLASS_POINT &s2,const CLASS_PLANE &plane,CLASS_POINT &clipped)
+{
+ GREAL _dis1,_dis2;
+ _dis1 = DISTANCE_PLANE_POINT(plane,s1);
+ VEC_DIFF(clipped,s2,s1);
+ _dis2 = VEC_DOT(clipped,plane);
+ VEC_SCALE(clipped,-_dis1/_dis2,clipped);
+ VEC_SUM(clipped,clipped,s1);
+}
+
+enum ePLANE_INTERSECTION_TYPE
+{
+ G_BACK_PLANE = 0,
+ G_COLLIDE_PLANE,
+ G_FRONT_PLANE
+};
+
+enum eLINE_PLANE_INTERSECTION_TYPE
+{
+ G_FRONT_PLANE_S1 = 0,
+ G_FRONT_PLANE_S2,
+ G_BACK_PLANE_S1,
+ G_BACK_PLANE_S2,
+ G_COLLIDE_PLANE_S1,
+ G_COLLIDE_PLANE_S2
+};
+
+//! Confirms if the plane intersect the edge or nor
+/*!
+intersection type must have the following values
+<ul>
+<li> 0 : Segment in front of plane, s1 closest
+<li> 1 : Segment in front of plane, s2 closest
+<li> 2 : Segment in back of plane, s1 closest
+<li> 3 : Segment in back of plane, s2 closest
+<li> 4 : Segment collides plane, s1 in back
+<li> 5 : Segment collides plane, s2 in back
+</ul>
+*/
+
+template<typename CLASS_POINT,typename CLASS_PLANE>
+SIMD_FORCE_INLINE eLINE_PLANE_INTERSECTION_TYPE PLANE_CLIP_SEGMENT2(
+ const CLASS_POINT& s1,
+ const CLASS_POINT &s2,
+ const CLASS_PLANE &plane,CLASS_POINT &clipped)
+{
+ GREAL _dis1 = DISTANCE_PLANE_POINT(plane,s1);
+ GREAL _dis2 = DISTANCE_PLANE_POINT(plane,s2);
+ if(_dis1 >-G_EPSILON && _dis2 >-G_EPSILON)
+ {
+ if(_dis1<_dis2) return G_FRONT_PLANE_S1;
+ return G_FRONT_PLANE_S2;
+ }
+ else if(_dis1 <G_EPSILON && _dis2 <G_EPSILON)
+ {
+ if(_dis1>_dis2) return G_BACK_PLANE_S1;
+ return G_BACK_PLANE_S2;
+ }
+
+ VEC_DIFF(clipped,s2,s1);
+ _dis2 = VEC_DOT(clipped,plane);
+ VEC_SCALE(clipped,-_dis1/_dis2,clipped);
+ VEC_SUM(clipped,clipped,s1);
+ if(_dis1<_dis2) return G_COLLIDE_PLANE_S1;
+ return G_COLLIDE_PLANE_S2;
+}
+
+//! Confirms if the plane intersect the edge or not
+/*!
+clipped1 and clipped2 are the vertices behind the plane.
+clipped1 is the closest
+
+intersection_type must have the following values
+<ul>
+<li> 0 : Segment in front of plane, s1 closest
+<li> 1 : Segment in front of plane, s2 closest
+<li> 2 : Segment in back of plane, s1 closest
+<li> 3 : Segment in back of plane, s2 closest
+<li> 4 : Segment collides plane, s1 in back
+<li> 5 : Segment collides plane, s2 in back
+</ul>
+*/
+template<typename CLASS_POINT,typename CLASS_PLANE>
+SIMD_FORCE_INLINE eLINE_PLANE_INTERSECTION_TYPE PLANE_CLIP_SEGMENT_CLOSEST(
+ const CLASS_POINT& s1,
+ const CLASS_POINT &s2,
+ const CLASS_PLANE &plane,
+ CLASS_POINT &clipped1,CLASS_POINT &clipped2)
+{
+ eLINE_PLANE_INTERSECTION_TYPE intersection_type = PLANE_CLIP_SEGMENT2(s1,s2,plane,clipped1);
+ switch(intersection_type)
+ {
+ case G_FRONT_PLANE_S1:
+ VEC_COPY(clipped1,s1);
+ VEC_COPY(clipped2,s2);
+ break;
+ case G_FRONT_PLANE_S2:
+ VEC_COPY(clipped1,s2);
+ VEC_COPY(clipped2,s1);
+ break;
+ case G_BACK_PLANE_S1:
+ VEC_COPY(clipped1,s1);
+ VEC_COPY(clipped2,s2);
+ break;
+ case G_BACK_PLANE_S2:
+ VEC_COPY(clipped1,s2);
+ VEC_COPY(clipped2,s1);
+ break;
+ case G_COLLIDE_PLANE_S1:
+ VEC_COPY(clipped2,s1);
+ break;
+ case G_COLLIDE_PLANE_S2:
+ VEC_COPY(clipped2,s2);
+ break;
+ }
+ return intersection_type;
+}
+
+
+//! Finds the 2 smallest cartesian coordinates of a plane normal
+#define PLANE_MINOR_AXES(plane, i0, i1) VEC_MINOR_AXES(plane, i0, i1)
+
+//! Ray plane collision in one way
+/*!
+Intersects plane in one way only. The ray must face the plane (normals must be in opossite directions).<br/>
+It uses the PLANEDIREPSILON constant.
+*/
+template<typename T,typename CLASS_POINT,typename CLASS_PLANE>
+SIMD_FORCE_INLINE bool RAY_PLANE_COLLISION(
+ const CLASS_PLANE & plane,
+ const CLASS_POINT & vDir,
+ const CLASS_POINT & vPoint,
+ CLASS_POINT & pout,T &tparam)
+{
+ GREAL _dis,_dotdir;
+ _dotdir = VEC_DOT(plane,vDir);
+ if(_dotdir<PLANEDIREPSILON)
+ {
+ return false;
+ }
+ _dis = DISTANCE_PLANE_POINT(plane,vPoint);
+ tparam = -_dis/_dotdir;
+ VEC_SCALE(pout,tparam,vDir);
+ VEC_SUM(pout,vPoint,pout);
+ return true;
+}
+
+//! line collision
+/*!
+*\return
+ -0 if the ray never intersects
+ -1 if the ray collides in front
+ -2 if the ray collides in back
+*/
+template<typename T,typename CLASS_POINT,typename CLASS_PLANE>
+SIMD_FORCE_INLINE GUINT LINE_PLANE_COLLISION(
+ const CLASS_PLANE & plane,
+ const CLASS_POINT & vDir,
+ const CLASS_POINT & vPoint,
+ CLASS_POINT & pout,
+ T &tparam,
+ T tmin, T tmax)
+{
+ GREAL _dis,_dotdir;
+ _dotdir = VEC_DOT(plane,vDir);
+ if(btFabs(_dotdir)<PLANEDIREPSILON)
+ {
+ tparam = tmax;
+ return 0;
+ }
+ _dis = DISTANCE_PLANE_POINT(plane,vPoint);
+ char returnvalue = _dis<0.0f?2:1;
+ tparam = -_dis/_dotdir;
+
+ if(tparam<tmin)
+ {
+ returnvalue = 0;
+ tparam = tmin;
+ }
+ else if(tparam>tmax)
+ {
+ returnvalue = 0;
+ tparam = tmax;
+ }
+
+ VEC_SCALE(pout,tparam,vDir);
+ VEC_SUM(pout,vPoint,pout);
+ return returnvalue;
+}
+
+/*! \brief Returns the Ray on which 2 planes intersect if they do.
+ Written by Rodrigo Hernandez on ODE convex collision
+
+ \param p1 Plane 1
+ \param p2 Plane 2
+ \param p Contains the origin of the ray upon returning if planes intersect
+ \param d Contains the direction of the ray upon returning if planes intersect
+ \return true if the planes intersect, 0 if paralell.
+
+*/
+template<typename CLASS_POINT,typename CLASS_PLANE>
+SIMD_FORCE_INLINE bool INTERSECT_PLANES(
+ const CLASS_PLANE &p1,
+ const CLASS_PLANE &p2,
+ CLASS_POINT &p,
+ CLASS_POINT &d)
+{
+ VEC_CROSS(d,p1,p2);
+ GREAL denom = VEC_DOT(d, d);
+ if(GIM_IS_ZERO(denom)) return false;
+ vec3f _n;
+ _n[0]=p1[3]*p2[0] - p2[3]*p1[0];
+ _n[1]=p1[3]*p2[1] - p2[3]*p1[1];
+ _n[2]=p1[3]*p2[2] - p2[3]*p1[2];
+ VEC_CROSS(p,_n,d);
+ p[0]/=denom;
+ p[1]/=denom;
+ p[2]/=denom;
+ return true;
+}
+
+//***************** SEGMENT and LINE FUNCTIONS **********************************///
+
+/*! Finds the closest point(cp) to (v) on a segment (e1,e2)
+ */
+template<typename CLASS_POINT>
+SIMD_FORCE_INLINE void CLOSEST_POINT_ON_SEGMENT(
+ CLASS_POINT & cp, const CLASS_POINT & v,
+ const CLASS_POINT &e1,const CLASS_POINT &e2)
+{
+ vec3f _n;
+ VEC_DIFF(_n,e2,e1);
+ VEC_DIFF(cp,v,e1);
+ GREAL _scalar = VEC_DOT(cp, _n);
+ _scalar/= VEC_DOT(_n, _n);
+ if(_scalar <0.0f)
+ {
+ VEC_COPY(cp,e1);
+ }
+ else if(_scalar >1.0f)
+ {
+ VEC_COPY(cp,e2);
+ }
+ else
+ {
+ VEC_SCALE(cp,_scalar,_n);
+ VEC_SUM(cp,cp,e1);
+ }
+}
+
+
+/*! \brief Finds the line params where these lines intersect.
+
+\param dir1 Direction of line 1
+\param point1 Point of line 1
+\param dir2 Direction of line 2
+\param point2 Point of line 2
+\param t1 Result Parameter for line 1
+\param t2 Result Parameter for line 2
+\param dointersect 0 if the lines won't intersect, else 1
+
+*/
+template<typename T,typename CLASS_POINT>
+SIMD_FORCE_INLINE bool LINE_INTERSECTION_PARAMS(
+ const CLASS_POINT & dir1,
+ CLASS_POINT & point1,
+ const CLASS_POINT & dir2,
+ CLASS_POINT & point2,
+ T& t1,T& t2)
+{
+ GREAL det;
+ GREAL e1e1 = VEC_DOT(dir1,dir1);
+ GREAL e1e2 = VEC_DOT(dir1,dir2);
+ GREAL e2e2 = VEC_DOT(dir2,dir2);
+ vec3f p1p2;
+ VEC_DIFF(p1p2,point1,point2);
+ GREAL p1p2e1 = VEC_DOT(p1p2,dir1);
+ GREAL p1p2e2 = VEC_DOT(p1p2,dir2);
+ det = e1e2*e1e2 - e1e1*e2e2;
+ if(GIM_IS_ZERO(det)) return false;
+ t1 = (e1e2*p1p2e2 - e2e2*p1p2e1)/det;
+ t2 = (e1e1*p1p2e2 - e1e2*p1p2e1)/det;
+ return true;
+}
+
+//! Find closest points on segments
+template<typename CLASS_POINT>
+SIMD_FORCE_INLINE void SEGMENT_COLLISION(
+ const CLASS_POINT & vA1,
+ const CLASS_POINT & vA2,
+ const CLASS_POINT & vB1,
+ const CLASS_POINT & vB2,
+ CLASS_POINT & vPointA,
+ CLASS_POINT & vPointB)
+{
+ CLASS_POINT _AD,_BD,_N;
+ vec4f _M;//plane
+ VEC_DIFF(_AD,vA2,vA1);
+ VEC_DIFF(_BD,vB2,vB1);
+ VEC_CROSS(_N,_AD,_BD);
+ GREAL _tp = VEC_DOT(_N,_N);
+ if(_tp<G_EPSILON)//ARE PARALELE
+ {
+ //project B over A
+ bool invert_b_order = false;
+ _M[0] = VEC_DOT(vB1,_AD);
+ _M[1] = VEC_DOT(vB2,_AD);
+ if(_M[0]>_M[1])
+ {
+ invert_b_order = true;
+ GIM_SWAP_NUMBERS(_M[0],_M[1]);
+ }
+ _M[2] = VEC_DOT(vA1,_AD);
+ _M[3] = VEC_DOT(vA2,_AD);
+ //mid points
+ _N[0] = (_M[0]+_M[1])*0.5f;
+ _N[1] = (_M[2]+_M[3])*0.5f;
+
+ if(_N[0]<_N[1])
+ {
+ if(_M[1]<_M[2])
+ {
+ vPointB = invert_b_order?vB1:vB2;
+ vPointA = vA1;
+ }
+ else if(_M[1]<_M[3])
+ {
+ vPointB = invert_b_order?vB1:vB2;
+ CLOSEST_POINT_ON_SEGMENT(vPointA,vPointB,vA1,vA2);
+ }
+ else
+ {
+ vPointA = vA2;
+ CLOSEST_POINT_ON_SEGMENT(vPointB,vPointA,vB1,vB2);
+ }
+ }
+ else
+ {
+ if(_M[3]<_M[0])
+ {
+ vPointB = invert_b_order?vB2:vB1;
+ vPointA = vA2;
+ }
+ else if(_M[3]<_M[1])
+ {
+ vPointA = vA2;
+ CLOSEST_POINT_ON_SEGMENT(vPointB,vPointA,vB1,vB2);
+ }
+ else
+ {
+ vPointB = invert_b_order?vB1:vB2;
+ CLOSEST_POINT_ON_SEGMENT(vPointA,vPointB,vA1,vA2);
+ }
+ }
+ return;
+ }
+
+
+ VEC_CROSS(_M,_N,_BD);
+ _M[3] = VEC_DOT(_M,vB1);
+
+ LINE_PLANE_COLLISION(_M,_AD,vA1,vPointA,_tp,btScalar(0), btScalar(1));
+ /*Closest point on segment*/
+ VEC_DIFF(vPointB,vPointA,vB1);
+ _tp = VEC_DOT(vPointB, _BD);
+ _tp/= VEC_DOT(_BD, _BD);
+ _tp = GIM_CLAMP(_tp,0.0f,1.0f);
+ VEC_SCALE(vPointB,_tp,_BD);
+ VEC_SUM(vPointB,vPointB,vB1);
+}
+
+
+
+
+//! Line box intersection in one dimension
+/*!
+
+*\param pos Position of the ray
+*\param dir Projection of the Direction of the ray
+*\param bmin Minimum bound of the box
+*\param bmax Maximum bound of the box
+*\param tfirst the minimum projection. Assign to 0 at first.
+*\param tlast the maximum projection. Assign to INFINITY at first.
+*\return true if there is an intersection.
+*/
+template<typename T>
+SIMD_FORCE_INLINE bool BOX_AXIS_INTERSECT(T pos, T dir,T bmin, T bmax, T & tfirst, T & tlast)
+{
+ if(GIM_IS_ZERO(dir))
+ {
+ return !(pos < bmin || pos > bmax);
+ }
+ GREAL a0 = (bmin - pos) / dir;
+ GREAL a1 = (bmax - pos) / dir;
+ if(a0 > a1) GIM_SWAP_NUMBERS(a0, a1);
+ tfirst = GIM_MAX(a0, tfirst);
+ tlast = GIM_MIN(a1, tlast);
+ if (tlast < tfirst) return false;
+ return true;
+}
+
+
+//! Sorts 3 componets
+template<typename T>
+SIMD_FORCE_INLINE void SORT_3_INDICES(
+ const T * values,
+ GUINT * order_indices)
+{
+ //get minimum
+ order_indices[0] = values[0] < values[1] ? (values[0] < values[2] ? 0 : 2) : (values[1] < values[2] ? 1 : 2);
+
+ //get second and third
+ GUINT i0 = (order_indices[0] + 1)%3;
+ GUINT i1 = (i0 + 1)%3;
+
+ if(values[i0] < values[i1])
+ {
+ order_indices[1] = i0;
+ order_indices[2] = i1;
+ }
+ else
+ {
+ order_indices[1] = i1;
+ order_indices[2] = i0;
+ }
+}
+
+
+
+
+
+#endif // GIM_VECTOR_H_INCLUDED
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_bitset.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_bitset.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_bitset.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_bitset.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,123 @@
+#ifndef GIM_BITSET_H_INCLUDED
+#define GIM_BITSET_H_INCLUDED
+/*! \file gim_bitset.h
+\author Francisco Len Nßjera
+*/
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman at yahoo.com
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of EITHER:
+ (1) The GNU Lesser General Public License as published by the Free
+ Software Foundation; either version 2.1 of the License, or (at
+ your option) any later version. The text of the GNU Lesser
+ General Public License is included with this library in the
+ file GIMPACT-LICENSE-LGPL.TXT.
+ (2) The BSD-style license that is included with this library in
+ the file GIMPACT-LICENSE-BSD.TXT.
+ (3) The zlib/libpng license that is included with this library in
+ the file GIMPACT-LICENSE-ZLIB.TXT.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
+ GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+
+#include "gim_array.h"
+
+
+#define GUINT_BIT_COUNT 32
+#define GUINT_EXPONENT 5
+
+class gim_bitset
+{
+public:
+ gim_array<GUINT> m_container;
+
+ gim_bitset()
+ {
+
+ }
+
+ gim_bitset(GUINT bits_count)
+ {
+ resize(bits_count);
+ }
+
+ ~gim_bitset()
+ {
+ }
+
+ inline bool resize(GUINT newsize)
+ {
+ GUINT oldsize = m_container.size();
+ m_container.resize(newsize/GUINT_BIT_COUNT + 1,false);
+ while(oldsize<m_container.size())
+ {
+ m_container[oldsize] = 0;
+ }
+ return true;
+ }
+
+ inline GUINT size()
+ {
+ return m_container.size()*GUINT_BIT_COUNT;
+ }
+
+ inline void set_all()
+ {
+ for(GUINT i = 0;i<m_container.size();++i)
+ {
+ m_container[i] = 0xffffffff;
+ }
+ }
+
+ inline void clear_all()
+ {
+ for(GUINT i = 0;i<m_container.size();++i)
+ {
+ m_container[i] = 0;
+ }
+ }
+
+ inline void set(GUINT bit_index)
+ {
+ if(bit_index>=size())
+ {
+ resize(bit_index);
+ }
+ m_container[bit_index >> GUINT_EXPONENT] |= (1 << (bit_index & (GUINT_BIT_COUNT-1)));
+ }
+
+ ///Return 0 or 1
+ inline char get(GUINT bit_index)
+ {
+ if(bit_index>=size())
+ {
+ return 0;
+ }
+ char value = m_container[bit_index >> GUINT_EXPONENT] &
+ (1 << (bit_index & (GUINT_BIT_COUNT-1)));
+ return value;
+ }
+
+ inline void clear(GUINT bit_index)
+ {
+ m_container[bit_index >> GUINT_EXPONENT] &= ~(1 << (bit_index & (GUINT_BIT_COUNT-1)));
+ }
+};
+
+
+
+
+
+#endif // GIM_CONTAINERS_H_INCLUDED
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_box_collision.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_box_collision.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_box_collision.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_box_collision.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,590 @@
+#ifndef GIM_BOX_COLLISION_H_INCLUDED
+#define GIM_BOX_COLLISION_H_INCLUDED
+
+/*! \file gim_box_collision.h
+\author Francisco Len Nßjera
+*/
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman at yahoo.com
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of EITHER:
+ (1) The GNU Lesser General Public License as published by the Free
+ Software Foundation; either version 2.1 of the License, or (at
+ your option) any later version. The text of the GNU Lesser
+ General Public License is included with this library in the
+ file GIMPACT-LICENSE-LGPL.TXT.
+ (2) The BSD-style license that is included with this library in
+ the file GIMPACT-LICENSE-BSD.TXT.
+ (3) The zlib/libpng license that is included with this library in
+ the file GIMPACT-LICENSE-ZLIB.TXT.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
+ GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+#include "gim_basic_geometry_operations.h"
+#include "LinearMath/btTransform.h"
+
+
+
+//SIMD_FORCE_INLINE bool test_cross_edge_box(
+// const btVector3 & edge,
+// const btVector3 & absolute_edge,
+// const btVector3 & pointa,
+// const btVector3 & pointb, const btVector3 & extend,
+// int dir_index0,
+// int dir_index1
+// int component_index0,
+// int component_index1)
+//{
+// // dir coords are -z and y
+//
+// const btScalar dir0 = -edge[dir_index0];
+// const btScalar dir1 = edge[dir_index1];
+// btScalar pmin = pointa[component_index0]*dir0 + pointa[component_index1]*dir1;
+// btScalar pmax = pointb[component_index0]*dir0 + pointb[component_index1]*dir1;
+// //find minmax
+// if(pmin>pmax)
+// {
+// GIM_SWAP_NUMBERS(pmin,pmax);
+// }
+// //find extends
+// const btScalar rad = extend[component_index0] * absolute_edge[dir_index0] +
+// extend[component_index1] * absolute_edge[dir_index1];
+//
+// if(pmin>rad || -rad>pmax) return false;
+// return true;
+//}
+//
+//SIMD_FORCE_INLINE bool test_cross_edge_box_X_axis(
+// const btVector3 & edge,
+// const btVector3 & absolute_edge,
+// const btVector3 & pointa,
+// const btVector3 & pointb, btVector3 & extend)
+//{
+//
+// return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,2,1,1,2);
+//}
+//
+//
+//SIMD_FORCE_INLINE bool test_cross_edge_box_Y_axis(
+// const btVector3 & edge,
+// const btVector3 & absolute_edge,
+// const btVector3 & pointa,
+// const btVector3 & pointb, btVector3 & extend)
+//{
+//
+// return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,0,2,2,0);
+//}
+//
+//SIMD_FORCE_INLINE bool test_cross_edge_box_Z_axis(
+// const btVector3 & edge,
+// const btVector3 & absolute_edge,
+// const btVector3 & pointa,
+// const btVector3 & pointb, btVector3 & extend)
+//{
+//
+// return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,1,0,0,1);
+//}
+
+#define TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,i_dir_0,i_dir_1,i_comp_0,i_comp_1)\
+{\
+ const btScalar dir0 = -edge[i_dir_0];\
+ const btScalar dir1 = edge[i_dir_1];\
+ btScalar pmin = pointa[i_comp_0]*dir0 + pointa[i_comp_1]*dir1;\
+ btScalar pmax = pointb[i_comp_0]*dir0 + pointb[i_comp_1]*dir1;\
+ if(pmin>pmax)\
+ {\
+ GIM_SWAP_NUMBERS(pmin,pmax); \
+ }\
+ const btScalar abs_dir0 = absolute_edge[i_dir_0];\
+ const btScalar abs_dir1 = absolute_edge[i_dir_1];\
+ const btScalar rad = _extend[i_comp_0] * abs_dir0 + _extend[i_comp_1] * abs_dir1;\
+ if(pmin>rad || -rad>pmax) return false;\
+}\
+
+
+#define TEST_CROSS_EDGE_BOX_X_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\
+{\
+ TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,2,1,1,2);\
+}\
+
+#define TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\
+{\
+ TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,0,2,2,0);\
+}\
+
+#define TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\
+{\
+ TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,1,0,0,1);\
+}\
+
+
+
+//! Class for transforming a model1 to the space of model0
+class GIM_BOX_BOX_TRANSFORM_CACHE
+{
+public:
+ btVector3 m_T1to0;//!< Transforms translation of model1 to model 0
+ btMatrix3x3 m_R1to0;//!< Transforms Rotation of model1 to model 0, equal to R0' * R1
+ btMatrix3x3 m_AR;//!< Absolute value of m_R1to0
+
+ SIMD_FORCE_INLINE void calc_absolute_matrix()
+ {
+ static const btVector3 vepsi(1e-6f,1e-6f,1e-6f);
+ m_AR[0] = vepsi + m_R1to0[0].absolute();
+ m_AR[1] = vepsi + m_R1to0[1].absolute();
+ m_AR[2] = vepsi + m_R1to0[2].absolute();
+ }
+
+ GIM_BOX_BOX_TRANSFORM_CACHE()
+ {
+ }
+
+
+ GIM_BOX_BOX_TRANSFORM_CACHE(mat4f trans1_to_0)
+ {
+ COPY_MATRIX_3X3(m_R1to0,trans1_to_0)
+ MAT_GET_TRANSLATION(trans1_to_0,m_T1to0)
+ calc_absolute_matrix();
+ }
+
+ //! Calc the transformation relative 1 to 0. Inverts matrics by transposing
+ SIMD_FORCE_INLINE void calc_from_homogenic(const btTransform & trans0,const btTransform & trans1)
+ {
+
+ m_R1to0 = trans0.getBasis().transpose();
+ m_T1to0 = m_R1to0 * (-trans0.getOrigin());
+
+ m_T1to0 += m_R1to0*trans1.getOrigin();
+ m_R1to0 *= trans1.getBasis();
+
+ calc_absolute_matrix();
+ }
+
+ //! Calcs the full invertion of the matrices. Useful for scaling matrices
+ SIMD_FORCE_INLINE void calc_from_full_invert(const btTransform & trans0,const btTransform & trans1)
+ {
+ m_R1to0 = trans0.getBasis().inverse();
+ m_T1to0 = m_R1to0 * (-trans0.getOrigin());
+
+ m_T1to0 += m_R1to0*trans1.getOrigin();
+ m_R1to0 *= trans1.getBasis();
+
+ calc_absolute_matrix();
+ }
+
+ SIMD_FORCE_INLINE btVector3 transform(const btVector3 & point)
+ {
+ return btVector3(m_R1to0[0].dot(point) + m_T1to0.x(),
+ m_R1to0[1].dot(point) + m_T1to0.y(),
+ m_R1to0[2].dot(point) + m_T1to0.z());
+ }
+};
+
+
+#define BOX_PLANE_EPSILON 0.000001f
+
+//! Axis aligned box
+class GIM_AABB
+{
+public:
+ btVector3 m_min;
+ btVector3 m_max;
+
+ GIM_AABB()
+ {}
+
+
+ GIM_AABB(const btVector3 & V1,
+ const btVector3 & V2,
+ const btVector3 & V3)
+ {
+ m_min[0] = GIM_MIN3(V1[0],V2[0],V3[0]);
+ m_min[1] = GIM_MIN3(V1[1],V2[1],V3[1]);
+ m_min[2] = GIM_MIN3(V1[2],V2[2],V3[2]);
+
+ m_max[0] = GIM_MAX3(V1[0],V2[0],V3[0]);
+ m_max[1] = GIM_MAX3(V1[1],V2[1],V3[1]);
+ m_max[2] = GIM_MAX3(V1[2],V2[2],V3[2]);
+ }
+
+ GIM_AABB(const btVector3 & V1,
+ const btVector3 & V2,
+ const btVector3 & V3,
+ GREAL margin)
+ {
+ m_min[0] = GIM_MIN3(V1[0],V2[0],V3[0]);
+ m_min[1] = GIM_MIN3(V1[1],V2[1],V3[1]);
+ m_min[2] = GIM_MIN3(V1[2],V2[2],V3[2]);
+
+ m_max[0] = GIM_MAX3(V1[0],V2[0],V3[0]);
+ m_max[1] = GIM_MAX3(V1[1],V2[1],V3[1]);
+ m_max[2] = GIM_MAX3(V1[2],V2[2],V3[2]);
+
+ m_min[0] -= margin;
+ m_min[1] -= margin;
+ m_min[2] -= margin;
+ m_max[0] += margin;
+ m_max[1] += margin;
+ m_max[2] += margin;
+ }
+
+ GIM_AABB(const GIM_AABB &other):
+ m_min(other.m_min),m_max(other.m_max)
+ {
+ }
+
+ GIM_AABB(const GIM_AABB &other,btScalar margin ):
+ m_min(other.m_min),m_max(other.m_max)
+ {
+ m_min[0] -= margin;
+ m_min[1] -= margin;
+ m_min[2] -= margin;
+ m_max[0] += margin;
+ m_max[1] += margin;
+ m_max[2] += margin;
+ }
+
+ SIMD_FORCE_INLINE void invalidate()
+ {
+ m_min[0] = G_REAL_INFINITY;
+ m_min[1] = G_REAL_INFINITY;
+ m_min[2] = G_REAL_INFINITY;
+ m_max[0] = -G_REAL_INFINITY;
+ m_max[1] = -G_REAL_INFINITY;
+ m_max[2] = -G_REAL_INFINITY;
+ }
+
+ SIMD_FORCE_INLINE void increment_margin(btScalar margin)
+ {
+ m_min[0] -= margin;
+ m_min[1] -= margin;
+ m_min[2] -= margin;
+ m_max[0] += margin;
+ m_max[1] += margin;
+ m_max[2] += margin;
+ }
+
+ SIMD_FORCE_INLINE void copy_with_margin(const GIM_AABB &other, btScalar margin)
+ {
+ m_min[0] = other.m_min[0] - margin;
+ m_min[1] = other.m_min[1] - margin;
+ m_min[2] = other.m_min[2] - margin;
+
+ m_max[0] = other.m_max[0] + margin;
+ m_max[1] = other.m_max[1] + margin;
+ m_max[2] = other.m_max[2] + margin;
+ }
+
+ template<typename CLASS_POINT>
+ SIMD_FORCE_INLINE void calc_from_triangle(
+ const CLASS_POINT & V1,
+ const CLASS_POINT & V2,
+ const CLASS_POINT & V3)
+ {
+ m_min[0] = GIM_MIN3(V1[0],V2[0],V3[0]);
+ m_min[1] = GIM_MIN3(V1[1],V2[1],V3[1]);
+ m_min[2] = GIM_MIN3(V1[2],V2[2],V3[2]);
+
+ m_max[0] = GIM_MAX3(V1[0],V2[0],V3[0]);
+ m_max[1] = GIM_MAX3(V1[1],V2[1],V3[1]);
+ m_max[2] = GIM_MAX3(V1[2],V2[2],V3[2]);
+ }
+
+ template<typename CLASS_POINT>
+ SIMD_FORCE_INLINE void calc_from_triangle_margin(
+ const CLASS_POINT & V1,
+ const CLASS_POINT & V2,
+ const CLASS_POINT & V3, btScalar margin)
+ {
+ m_min[0] = GIM_MIN3(V1[0],V2[0],V3[0]);
+ m_min[1] = GIM_MIN3(V1[1],V2[1],V3[1]);
+ m_min[2] = GIM_MIN3(V1[2],V2[2],V3[2]);
+
+ m_max[0] = GIM_MAX3(V1[0],V2[0],V3[0]);
+ m_max[1] = GIM_MAX3(V1[1],V2[1],V3[1]);
+ m_max[2] = GIM_MAX3(V1[2],V2[2],V3[2]);
+
+ m_min[0] -= margin;
+ m_min[1] -= margin;
+ m_min[2] -= margin;
+ m_max[0] += margin;
+ m_max[1] += margin;
+ m_max[2] += margin;
+ }
+
+ //! Apply a transform to an AABB
+ SIMD_FORCE_INLINE void appy_transform(const btTransform & trans)
+ {
+ btVector3 center = (m_max+m_min)*0.5f;
+ btVector3 extends = m_max - center;
+ // Compute new center
+ center = trans(center);
+
+ btVector3 textends(extends.dot(trans.getBasis().getRow(0).absolute()),
+ extends.dot(trans.getBasis().getRow(1).absolute()),
+ extends.dot(trans.getBasis().getRow(2).absolute()));
+
+ m_min = center - textends;
+ m_max = center + textends;
+ }
+
+ //! Merges a Box
+ SIMD_FORCE_INLINE void merge(const GIM_AABB & box)
+ {
+ m_min[0] = GIM_MIN(m_min[0],box.m_min[0]);
+ m_min[1] = GIM_MIN(m_min[1],box.m_min[1]);
+ m_min[2] = GIM_MIN(m_min[2],box.m_min[2]);
+
+ m_max[0] = GIM_MAX(m_max[0],box.m_max[0]);
+ m_max[1] = GIM_MAX(m_max[1],box.m_max[1]);
+ m_max[2] = GIM_MAX(m_max[2],box.m_max[2]);
+ }
+
+ //! Merges a point
+ template<typename CLASS_POINT>
+ SIMD_FORCE_INLINE void merge_point(const CLASS_POINT & point)
+ {
+ m_min[0] = GIM_MIN(m_min[0],point[0]);
+ m_min[1] = GIM_MIN(m_min[1],point[1]);
+ m_min[2] = GIM_MIN(m_min[2],point[2]);
+
+ m_max[0] = GIM_MAX(m_max[0],point[0]);
+ m_max[1] = GIM_MAX(m_max[1],point[1]);
+ m_max[2] = GIM_MAX(m_max[2],point[2]);
+ }
+
+ //! Gets the extend and center
+ SIMD_FORCE_INLINE void get_center_extend(btVector3 & center,btVector3 & extend) const
+ {
+ center = (m_max+m_min)*0.5f;
+ extend = m_max - center;
+ }
+
+ //! Finds the intersecting box between this box and the other.
+ SIMD_FORCE_INLINE void find_intersection(const GIM_AABB & other, GIM_AABB & intersection) const
+ {
+ intersection.m_min[0] = GIM_MAX(other.m_min[0],m_min[0]);
+ intersection.m_min[1] = GIM_MAX(other.m_min[1],m_min[1]);
+ intersection.m_min[2] = GIM_MAX(other.m_min[2],m_min[2]);
+
+ intersection.m_max[0] = GIM_MIN(other.m_max[0],m_max[0]);
+ intersection.m_max[1] = GIM_MIN(other.m_max[1],m_max[1]);
+ intersection.m_max[2] = GIM_MIN(other.m_max[2],m_max[2]);
+ }
+
+
+ SIMD_FORCE_INLINE bool has_collision(const GIM_AABB & other) const
+ {
+ if(m_min[0] > other.m_max[0] ||
+ m_max[0] < other.m_min[0] ||
+ m_min[1] > other.m_max[1] ||
+ m_max[1] < other.m_min[1] ||
+ m_min[2] > other.m_max[2] ||
+ m_max[2] < other.m_min[2])
+ {
+ return false;
+ }
+ return true;
+ }
+
+ /*! \brief Finds the Ray intersection parameter.
+ \param aabb Aligned box
+ \param vorigin A vec3f with the origin of the ray
+ \param vdir A vec3f with the direction of the ray
+ */
+ SIMD_FORCE_INLINE bool collide_ray(const btVector3 & vorigin,const btVector3 & vdir)
+ {
+ btVector3 extents,center;
+ this->get_center_extend(center,extents);;
+
+ btScalar Dx = vorigin[0] - center[0];
+ if(GIM_GREATER(Dx, extents[0]) && Dx*vdir[0]>=0.0f) return false;
+ btScalar Dy = vorigin[1] - center[1];
+ if(GIM_GREATER(Dy, extents[1]) && Dy*vdir[1]>=0.0f) return false;
+ btScalar Dz = vorigin[2] - center[2];
+ if(GIM_GREATER(Dz, extents[2]) && Dz*vdir[2]>=0.0f) return false;
+
+
+ btScalar f = vdir[1] * Dz - vdir[2] * Dy;
+ if(btFabs(f) > extents[1]*btFabs(vdir[2]) + extents[2]*btFabs(vdir[1])) return false;
+ f = vdir[2] * Dx - vdir[0] * Dz;
+ if(btFabs(f) > extents[0]*btFabs(vdir[2]) + extents[2]*btFabs(vdir[0]))return false;
+ f = vdir[0] * Dy - vdir[1] * Dx;
+ if(btFabs(f) > extents[0]*btFabs(vdir[1]) + extents[1]*btFabs(vdir[0]))return false;
+ return true;
+ }
+
+
+ SIMD_FORCE_INLINE void projection_interval(const btVector3 & direction, btScalar &vmin, btScalar &vmax) const
+ {
+ btVector3 center = (m_max+m_min)*0.5f;
+ btVector3 extend = m_max-center;
+
+ btScalar _fOrigin = direction.dot(center);
+ btScalar _fMaximumExtent = extend.dot(direction.absolute());
+ vmin = _fOrigin - _fMaximumExtent;
+ vmax = _fOrigin + _fMaximumExtent;
+ }
+
+ SIMD_FORCE_INLINE ePLANE_INTERSECTION_TYPE plane_classify(const btVector4 &plane) const
+ {
+ btScalar _fmin,_fmax;
+ this->projection_interval(plane,_fmin,_fmax);
+
+ if(plane[3] > _fmax + BOX_PLANE_EPSILON)
+ {
+ return G_BACK_PLANE; // 0
+ }
+
+ if(plane[3]+BOX_PLANE_EPSILON >=_fmin)
+ {
+ return G_COLLIDE_PLANE; //1
+ }
+ return G_FRONT_PLANE;//2
+ }
+
+ SIMD_FORCE_INLINE bool overlapping_trans_conservative(const GIM_AABB & box, btTransform & trans1_to_0)
+ {
+ GIM_AABB tbox = box;
+ tbox.appy_transform(trans1_to_0);
+ return has_collision(tbox);
+ }
+
+ //! transcache is the transformation cache from box to this AABB
+ SIMD_FORCE_INLINE bool overlapping_trans_cache(
+ const GIM_AABB & box,const GIM_BOX_BOX_TRANSFORM_CACHE & transcache, bool fulltest)
+ {
+
+ //Taken from OPCODE
+ btVector3 ea,eb;//extends
+ btVector3 ca,cb;//extends
+ get_center_extend(ca,ea);
+ box.get_center_extend(cb,eb);
+
+
+ btVector3 T;
+ btScalar t,t2;
+ int i;
+
+ // Class I : A's basis vectors
+ for(i=0;i<3;i++)
+ {
+ T[i] = transcache.m_R1to0[i].dot(cb) + transcache.m_T1to0[i] - ca[i];
+ t = transcache.m_AR[i].dot(eb) + ea[i];
+ if(GIM_GREATER(T[i], t)) return false;
+ }
+ // Class II : B's basis vectors
+ for(i=0;i<3;i++)
+ {
+ t = MAT_DOT_COL(transcache.m_R1to0,T,i);
+ t2 = MAT_DOT_COL(transcache.m_AR,ea,i) + eb[i];
+ if(GIM_GREATER(t,t2)) return false;
+ }
+ // Class III : 9 cross products
+ if(fulltest)
+ {
+ int j,m,n,o,p,q,r;
+ for(i=0;i<3;i++)
+ {
+ m = (i+1)%3;
+ n = (i+2)%3;
+ o = i==0?1:0;
+ p = i==2?1:2;
+ for(j=0;j<3;j++)
+ {
+ q = j==2?1:2;
+ r = j==0?1:0;
+ t = T[n]*transcache.m_R1to0[m][j] - T[m]*transcache.m_R1to0[n][j];
+ t2 = ea[o]*transcache.m_AR[p][j] + ea[p]*transcache.m_AR[o][j] +
+ eb[r]*transcache.m_AR[i][q] + eb[q]*transcache.m_AR[i][r];
+ if(GIM_GREATER(t,t2)) return false;
+ }
+ }
+ }
+ return true;
+ }
+
+ //! Simple test for planes.
+ SIMD_FORCE_INLINE bool collide_plane(
+ const btVector4 & plane)
+ {
+ ePLANE_INTERSECTION_TYPE classify = plane_classify(plane);
+ return (classify == G_COLLIDE_PLANE);
+ }
+
+ //! test for a triangle, with edges
+ SIMD_FORCE_INLINE bool collide_triangle_exact(
+ const btVector3 & p1,
+ const btVector3 & p2,
+ const btVector3 & p3,
+ const btVector4 & triangle_plane)
+ {
+ if(!collide_plane(triangle_plane)) return false;
+
+ btVector3 center,extends;
+ this->get_center_extend(center,extends);
+
+ const btVector3 v1(p1 - center);
+ const btVector3 v2(p2 - center);
+ const btVector3 v3(p3 - center);
+
+ //First axis
+ btVector3 diff(v2 - v1);
+ btVector3 abs_diff = diff.absolute();
+ //Test With X axis
+ TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v1,v3,extends);
+ //Test With Y axis
+ TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v1,v3,extends);
+ //Test With Z axis
+ TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v1,v3,extends);
+
+
+ diff = v3 - v2;
+ abs_diff = diff.absolute();
+ //Test With X axis
+ TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v2,v1,extends);
+ //Test With Y axis
+ TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v2,v1,extends);
+ //Test With Z axis
+ TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v2,v1,extends);
+
+ diff = v1 - v3;
+ abs_diff = diff.absolute();
+ //Test With X axis
+ TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v3,v2,extends);
+ //Test With Y axis
+ TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v3,v2,extends);
+ //Test With Z axis
+ TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v3,v2,extends);
+
+ return true;
+ }
+};
+
+
+//! Compairison of transformation objects
+SIMD_FORCE_INLINE bool btCompareTransformsEqual(const btTransform & t1,const btTransform & t2)
+{
+ if(!(t1.getOrigin() == t2.getOrigin()) ) return false;
+
+ if(!(t1.getBasis().getRow(0) == t2.getBasis().getRow(0)) ) return false;
+ if(!(t1.getBasis().getRow(1) == t2.getBasis().getRow(1)) ) return false;
+ if(!(t1.getBasis().getRow(2) == t2.getBasis().getRow(2)) ) return false;
+ return true;
+}
+
+
+
+#endif // GIM_BOX_COLLISION_H_INCLUDED
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_box_set.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_box_set.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_box_set.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_box_set.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,671 @@
+#ifndef GIM_BOX_SET_H_INCLUDED
+#define GIM_BOX_SET_H_INCLUDED
+
+/*! \file gim_box_set.h
+\author Francisco Len Nßjera
+*/
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman at yahoo.com
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of EITHER:
+ (1) The GNU Lesser General Public License as published by the Free
+ Software Foundation; either version 2.1 of the License, or (at
+ your option) any later version. The text of the GNU Lesser
+ General Public License is included with this library in the
+ file GIMPACT-LICENSE-LGPL.TXT.
+ (2) The BSD-style license that is included with this library in
+ the file GIMPACT-LICENSE-BSD.TXT.
+ (3) The zlib/libpng license that is included with this library in
+ the file GIMPACT-LICENSE-ZLIB.TXT.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
+ GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+
+
+#include "gim_array.h"
+#include "gim_radixsort.h"
+#include "gim_box_collision.h"
+#include "gim_tri_collision.h"
+
+
+
+//! Overlapping pair
+struct GIM_PAIR
+{
+ GUINT m_index1;
+ GUINT m_index2;
+ GIM_PAIR()
+ {}
+
+ GIM_PAIR(const GIM_PAIR & p)
+ {
+ m_index1 = p.m_index1;
+ m_index2 = p.m_index2;
+ }
+
+ GIM_PAIR(GUINT index1, GUINT index2)
+ {
+ m_index1 = index1;
+ m_index2 = index2;
+ }
+};
+
+//! A pairset array
+class gim_pair_set: public gim_array<GIM_PAIR>
+{
+public:
+ gim_pair_set():gim_array<GIM_PAIR>(32)
+ {
+ }
+ inline void push_pair(GUINT index1,GUINT index2)
+ {
+ push_back(GIM_PAIR(index1,index2));
+ }
+
+ inline void push_pair_inv(GUINT index1,GUINT index2)
+ {
+ push_back(GIM_PAIR(index2,index1));
+ }
+};
+
+
+//! Prototype Base class for primitive classification
+/*!
+This class is a wrapper for primitive collections.
+This tells relevant info for the Bounding Box set classes, which take care of space classification.
+This class can manage Compound shapes and trimeshes, and if it is managing trimesh then the Hierarchy Bounding Box classes will take advantage of primitive Vs Box overlapping tests for getting optimal results and less Per Box compairisons.
+*/
+class GIM_PRIMITIVE_MANAGER_PROTOTYPE
+{
+public:
+
+ //! determines if this manager consist on only triangles, which special case will be optimized
+ virtual bool is_trimesh() = 0;
+ virtual GUINT get_primitive_count() = 0;
+ virtual void get_primitive_box(GUINT prim_index ,GIM_AABB & primbox) = 0;
+ virtual void get_primitive_triangle(GUINT prim_index,GIM_TRIANGLE & triangle) = 0;
+};
+
+
+struct GIM_AABB_DATA
+{
+ GIM_AABB m_bound;
+ GUINT m_data;
+};
+
+//! Node Structure for trees
+struct GIM_BOX_TREE_NODE
+{
+ GIM_AABB m_bound;
+ GUINT m_left;//!< Left subtree
+ GUINT m_right;//!< Right subtree
+ GUINT m_escapeIndex;//!< Scape index for traversing
+ GUINT m_data;//!< primitive index if apply
+
+ GIM_BOX_TREE_NODE()
+ {
+ m_left = 0;
+ m_right = 0;
+ m_escapeIndex = 0;
+ m_data = 0;
+ }
+
+ SIMD_FORCE_INLINE bool is_leaf_node() const
+ {
+ return (!m_left && !m_right);
+ }
+};
+
+//! Basic Box tree structure
+class GIM_BOX_TREE
+{
+protected:
+ GUINT m_num_nodes;
+ gim_array<GIM_BOX_TREE_NODE> m_node_array;
+protected:
+ GUINT _sort_and_calc_splitting_index(
+ gim_array<GIM_AABB_DATA> & primitive_boxes,
+ GUINT startIndex, GUINT endIndex, GUINT splitAxis);
+
+ GUINT _calc_splitting_axis(gim_array<GIM_AABB_DATA> & primitive_boxes, GUINT startIndex, GUINT endIndex);
+
+ void _build_sub_tree(gim_array<GIM_AABB_DATA> & primitive_boxes, GUINT startIndex, GUINT endIndex);
+public:
+ GIM_BOX_TREE()
+ {
+ m_num_nodes = 0;
+ }
+
+ //! prototype functions for box tree management
+ //!@{
+ void build_tree(gim_array<GIM_AABB_DATA> & primitive_boxes);
+
+ SIMD_FORCE_INLINE void clearNodes()
+ {
+ m_node_array.clear();
+ m_num_nodes = 0;
+ }
+
+ //! node count
+ SIMD_FORCE_INLINE GUINT getNodeCount() const
+ {
+ return m_num_nodes;
+ }
+
+ //! tells if the node is a leaf
+ SIMD_FORCE_INLINE bool isLeafNode(GUINT nodeindex) const
+ {
+ return m_node_array[nodeindex].is_leaf_node();
+ }
+
+ SIMD_FORCE_INLINE GUINT getNodeData(GUINT nodeindex) const
+ {
+ return m_node_array[nodeindex].m_data;
+ }
+
+ SIMD_FORCE_INLINE void getNodeBound(GUINT nodeindex, GIM_AABB & bound) const
+ {
+ bound = m_node_array[nodeindex].m_bound;
+ }
+
+ SIMD_FORCE_INLINE void setNodeBound(GUINT nodeindex, const GIM_AABB & bound)
+ {
+ m_node_array[nodeindex].m_bound = bound;
+ }
+
+ SIMD_FORCE_INLINE GUINT getLeftNodeIndex(GUINT nodeindex) const
+ {
+ return m_node_array[nodeindex].m_left;
+ }
+
+ SIMD_FORCE_INLINE GUINT getRightNodeIndex(GUINT nodeindex) const
+ {
+ return m_node_array[nodeindex].m_right;
+ }
+
+ SIMD_FORCE_INLINE GUINT getScapeNodeIndex(GUINT nodeindex) const
+ {
+ return m_node_array[nodeindex].m_escapeIndex;
+ }
+
+ //!@}
+};
+
+
+//! Generic Box Tree Template
+/*!
+This class offers an structure for managing a box tree of primitives.
+Requires a Primitive prototype (like GIM_PRIMITIVE_MANAGER_PROTOTYPE ) and
+a Box tree structure ( like GIM_BOX_TREE).
+*/
+template<typename _GIM_PRIMITIVE_MANAGER_PROTOTYPE, typename _GIM_BOX_TREE_PROTOTYPE>
+class GIM_BOX_TREE_TEMPLATE_SET
+{
+protected:
+ _GIM_PRIMITIVE_MANAGER_PROTOTYPE m_primitive_manager;
+ _GIM_BOX_TREE_PROTOTYPE m_box_tree;
+protected:
+ //stackless refit
+ SIMD_FORCE_INLINE void refit()
+ {
+ GUINT nodecount = getNodeCount();
+ while(nodecount--)
+ {
+ if(isLeafNode(nodecount))
+ {
+ GIM_AABB leafbox;
+ m_primitive_manager.get_primitive_box(getNodeData(nodecount),leafbox);
+ setNodeBound(nodecount,leafbox);
+ }
+ else
+ {
+ //get left bound
+ GUINT childindex = getLeftNodeIndex(nodecount);
+ GIM_AABB bound;
+ getNodeBound(childindex,bound);
+ //get right bound
+ childindex = getRightNodeIndex(nodecount);
+ GIM_AABB bound2;
+ getNodeBound(childindex,bound2);
+ bound.merge(bound2);
+
+ setNodeBound(nodecount,bound);
+ }
+ }
+ }
+public:
+
+ GIM_BOX_TREE_TEMPLATE_SET()
+ {
+ }
+
+ SIMD_FORCE_INLINE GIM_AABB getGlobalBox() const
+ {
+ GIM_AABB totalbox;
+ getNodeBound(0, totalbox);
+ return totalbox;
+ }
+
+ SIMD_FORCE_INLINE void setPrimitiveManager(const _GIM_PRIMITIVE_MANAGER_PROTOTYPE & primitive_manager)
+ {
+ m_primitive_manager = primitive_manager;
+ }
+
+ const _GIM_PRIMITIVE_MANAGER_PROTOTYPE & getPrimitiveManager() const
+ {
+ return m_primitive_manager;
+ }
+
+ _GIM_PRIMITIVE_MANAGER_PROTOTYPE & getPrimitiveManager()
+ {
+ return m_primitive_manager;
+ }
+
+//! node manager prototype functions
+///@{
+
+ //! this attemps to refit the box set.
+ SIMD_FORCE_INLINE void update()
+ {
+ refit();
+ }
+
+ //! this rebuild the entire set
+ SIMD_FORCE_INLINE void buildSet()
+ {
+ //obtain primitive boxes
+ gim_array<GIM_AABB_DATA> primitive_boxes;
+ primitive_boxes.resize(m_primitive_manager.get_primitive_count(),false);
+
+ for (GUINT i = 0;i<primitive_boxes.size() ;i++ )
+ {
+ m_primitive_manager.get_primitive_box(i,primitive_boxes[i].m_bound);
+ primitive_boxes[i].m_data = i;
+ }
+
+ m_box_tree.build_tree(primitive_boxes);
+ }
+
+ //! returns the indices of the primitives in the m_primitive_manager
+ SIMD_FORCE_INLINE bool boxQuery(const GIM_AABB & box, gim_array<GUINT> & collided_results) const
+ {
+ GUINT curIndex = 0;
+ GUINT numNodes = getNodeCount();
+
+ while (curIndex < numNodes)
+ {
+ GIM_AABB bound;
+ getNodeBound(curIndex,bound);
+
+ //catch bugs in tree data
+
+ bool aabbOverlap = bound.has_collision(box);
+ bool isleafnode = isLeafNode(curIndex);
+
+ if (isleafnode && aabbOverlap)
+ {
+ collided_results.push_back(getNodeData(curIndex));
+ }
+
+ if (aabbOverlap || isleafnode)
+ {
+ //next subnode
+ curIndex++;
+ }
+ else
+ {
+ //skip node
+ curIndex+= getScapeNodeIndex(curIndex);
+ }
+ }
+ if(collided_results.size()>0) return true;
+ return false;
+ }
+
+ //! returns the indices of the primitives in the m_primitive_manager
+ SIMD_FORCE_INLINE bool boxQueryTrans(const GIM_AABB & box,
+ const btTransform & transform, gim_array<GUINT> & collided_results) const
+ {
+ GIM_AABB transbox=box;
+ transbox.appy_transform(transform);
+ return boxQuery(transbox,collided_results);
+ }
+
+ //! returns the indices of the primitives in the m_primitive_manager
+ SIMD_FORCE_INLINE bool rayQuery(
+ const btVector3 & ray_dir,const btVector3 & ray_origin ,
+ gim_array<GUINT> & collided_results) const
+ {
+ GUINT curIndex = 0;
+ GUINT numNodes = getNodeCount();
+
+ while (curIndex < numNodes)
+ {
+ GIM_AABB bound;
+ getNodeBound(curIndex,bound);
+
+ //catch bugs in tree data
+
+ bool aabbOverlap = bound.collide_ray(ray_origin,ray_dir);
+ bool isleafnode = isLeafNode(curIndex);
+
+ if (isleafnode && aabbOverlap)
+ {
+ collided_results.push_back(getNodeData( curIndex));
+ }
+
+ if (aabbOverlap || isleafnode)
+ {
+ //next subnode
+ curIndex++;
+ }
+ else
+ {
+ //skip node
+ curIndex+= getScapeNodeIndex(curIndex);
+ }
+ }
+ if(collided_results.size()>0) return true;
+ return false;
+ }
+
+ //! tells if this set has hierarcht
+ SIMD_FORCE_INLINE bool hasHierarchy() const
+ {
+ return true;
+ }
+
+ //! tells if this set is a trimesh
+ SIMD_FORCE_INLINE bool isTrimesh() const
+ {
+ return m_primitive_manager.is_trimesh();
+ }
+
+ //! node count
+ SIMD_FORCE_INLINE GUINT getNodeCount() const
+ {
+ return m_box_tree.getNodeCount();
+ }
+
+ //! tells if the node is a leaf
+ SIMD_FORCE_INLINE bool isLeafNode(GUINT nodeindex) const
+ {
+ return m_box_tree.isLeafNode(nodeindex);
+ }
+
+ SIMD_FORCE_INLINE GUINT getNodeData(GUINT nodeindex) const
+ {
+ return m_box_tree.getNodeData(nodeindex);
+ }
+
+ SIMD_FORCE_INLINE void getNodeBound(GUINT nodeindex, GIM_AABB & bound) const
+ {
+ m_box_tree.getNodeBound(nodeindex, bound);
+ }
+
+ SIMD_FORCE_INLINE void setNodeBound(GUINT nodeindex, const GIM_AABB & bound)
+ {
+ m_box_tree.setNodeBound(nodeindex, bound);
+ }
+
+ SIMD_FORCE_INLINE GUINT getLeftNodeIndex(GUINT nodeindex) const
+ {
+ return m_box_tree.getLeftNodeIndex(nodeindex);
+ }
+
+ SIMD_FORCE_INLINE GUINT getRightNodeIndex(GUINT nodeindex) const
+ {
+ return m_box_tree.getRightNodeIndex(nodeindex);
+ }
+
+ SIMD_FORCE_INLINE GUINT getScapeNodeIndex(GUINT nodeindex) const
+ {
+ return m_box_tree.getScapeNodeIndex(nodeindex);
+ }
+
+ SIMD_FORCE_INLINE void getNodeTriangle(GUINT nodeindex,GIM_TRIANGLE & triangle) const
+ {
+ m_primitive_manager.get_primitive_triangle(getNodeData(nodeindex),triangle);
+ }
+
+};
+
+//! Class for Box Tree Sets
+/*!
+this has the GIM_BOX_TREE implementation for bounding boxes.
+*/
+template<typename _GIM_PRIMITIVE_MANAGER_PROTOTYPE>
+class GIM_BOX_TREE_SET: public GIM_BOX_TREE_TEMPLATE_SET< _GIM_PRIMITIVE_MANAGER_PROTOTYPE, GIM_BOX_TREE>
+{
+public:
+
+};
+
+
+
+
+
+/// GIM_BOX_SET collision methods
+template<typename BOX_SET_CLASS0,typename BOX_SET_CLASS1>
+class GIM_TREE_TREE_COLLIDER
+{
+public:
+ gim_pair_set * m_collision_pairs;
+ BOX_SET_CLASS0 * m_boxset0;
+ BOX_SET_CLASS1 * m_boxset1;
+ GUINT current_node0;
+ GUINT current_node1;
+ bool node0_is_leaf;
+ bool node1_is_leaf;
+ bool t0_is_trimesh;
+ bool t1_is_trimesh;
+ bool node0_has_triangle;
+ bool node1_has_triangle;
+ GIM_AABB m_box0;
+ GIM_AABB m_box1;
+ GIM_BOX_BOX_TRANSFORM_CACHE trans_cache_1to0;
+ btTransform trans_cache_0to1;
+ GIM_TRIANGLE m_tri0;
+ btVector4 m_tri0_plane;
+ GIM_TRIANGLE m_tri1;
+ btVector4 m_tri1_plane;
+
+
+public:
+ GIM_TREE_TREE_COLLIDER()
+ {
+ current_node0 = G_UINT_INFINITY;
+ current_node1 = G_UINT_INFINITY;
+ }
+protected:
+ SIMD_FORCE_INLINE void retrieve_node0_triangle(GUINT node0)
+ {
+ if(node0_has_triangle) return;
+ m_boxset0->getNodeTriangle(node0,m_tri0);
+ //transform triangle
+ m_tri0.m_vertices[0] = trans_cache_0to1(m_tri0.m_vertices[0]);
+ m_tri0.m_vertices[1] = trans_cache_0to1(m_tri0.m_vertices[1]);
+ m_tri0.m_vertices[2] = trans_cache_0to1(m_tri0.m_vertices[2]);
+ m_tri0.get_plane(m_tri0_plane);
+
+ node0_has_triangle = true;
+ }
+
+ SIMD_FORCE_INLINE void retrieve_node1_triangle(GUINT node1)
+ {
+ if(node1_has_triangle) return;
+ m_boxset1->getNodeTriangle(node1,m_tri1);
+ //transform triangle
+ m_tri1.m_vertices[0] = trans_cache_1to0.transform(m_tri1.m_vertices[0]);
+ m_tri1.m_vertices[1] = trans_cache_1to0.transform(m_tri1.m_vertices[1]);
+ m_tri1.m_vertices[2] = trans_cache_1to0.transform(m_tri1.m_vertices[2]);
+ m_tri1.get_plane(m_tri1_plane);
+
+ node1_has_triangle = true;
+ }
+
+ SIMD_FORCE_INLINE void retrieve_node0_info(GUINT node0)
+ {
+ if(node0 == current_node0) return;
+ m_boxset0->getNodeBound(node0,m_box0);
+ node0_is_leaf = m_boxset0->isLeafNode(node0);
+ node0_has_triangle = false;
+ current_node0 = node0;
+ }
+
+ SIMD_FORCE_INLINE void retrieve_node1_info(GUINT node1)
+ {
+ if(node1 == current_node1) return;
+ m_boxset1->getNodeBound(node1,m_box1);
+ node1_is_leaf = m_boxset1->isLeafNode(node1);
+ node1_has_triangle = false;
+ current_node1 = node1;
+ }
+
+ SIMD_FORCE_INLINE bool node_collision(GUINT node0 ,GUINT node1)
+ {
+ retrieve_node0_info(node0);
+ retrieve_node1_info(node1);
+ bool result = m_box0.overlapping_trans_cache(m_box1,trans_cache_1to0,true);
+ if(!result) return false;
+
+ if(t0_is_trimesh && node0_is_leaf)
+ {
+ //perform primitive vs box collision
+ retrieve_node0_triangle(node0);
+ //do triangle vs box collision
+ m_box1.increment_margin(m_tri0.m_margin);
+
+ result = m_box1.collide_triangle_exact(
+ m_tri0.m_vertices[0],m_tri0.m_vertices[1],m_tri0.m_vertices[2],m_tri0_plane);
+
+ m_box1.increment_margin(-m_tri0.m_margin);
+
+ if(!result) return false;
+ return true;
+ }
+ else if(t1_is_trimesh && node1_is_leaf)
+ {
+ //perform primitive vs box collision
+ retrieve_node1_triangle(node1);
+ //do triangle vs box collision
+ m_box0.increment_margin(m_tri1.m_margin);
+
+ result = m_box0.collide_triangle_exact(
+ m_tri1.m_vertices[0],m_tri1.m_vertices[1],m_tri1.m_vertices[2],m_tri1_plane);
+
+ m_box0.increment_margin(-m_tri1.m_margin);
+
+ if(!result) return false;
+ return true;
+ }
+ return true;
+ }
+
+ //stackless collision routine
+ void find_collision_pairs()
+ {
+ gim_pair_set stack_collisions;
+ stack_collisions.reserve(32);
+
+ //add the first pair
+ stack_collisions.push_pair(0,0);
+
+
+ while(stack_collisions.size())
+ {
+ //retrieve the last pair and pop
+ GUINT node0 = stack_collisions.back().m_index1;
+ GUINT node1 = stack_collisions.back().m_index2;
+ stack_collisions.pop_back();
+ if(node_collision(node0,node1)) // a collision is found
+ {
+ if(node0_is_leaf)
+ {
+ if(node1_is_leaf)
+ {
+ m_collision_pairs->push_pair(m_boxset0->getNodeData(node0),m_boxset1->getNodeData(node1));
+ }
+ else
+ {
+ //collide left
+ stack_collisions.push_pair(node0,m_boxset1->getLeftNodeIndex(node1));
+
+ //collide right
+ stack_collisions.push_pair(node0,m_boxset1->getRightNodeIndex(node1));
+ }
+ }
+ else
+ {
+ if(node1_is_leaf)
+ {
+ //collide left
+ stack_collisions.push_pair(m_boxset0->getLeftNodeIndex(node0),node1);
+ //collide right
+ stack_collisions.push_pair(m_boxset0->getRightNodeIndex(node0),node1);
+ }
+ else
+ {
+ GUINT left0 = m_boxset0->getLeftNodeIndex(node0);
+ GUINT right0 = m_boxset0->getRightNodeIndex(node0);
+ GUINT left1 = m_boxset1->getLeftNodeIndex(node1);
+ GUINT right1 = m_boxset1->getRightNodeIndex(node1);
+ //collide left
+ stack_collisions.push_pair(left0,left1);
+ //collide right
+ stack_collisions.push_pair(left0,right1);
+ //collide left
+ stack_collisions.push_pair(right0,left1);
+ //collide right
+ stack_collisions.push_pair(right0,right1);
+
+ }// else if node1 is not a leaf
+ }// else if node0 is not a leaf
+
+ }// if(node_collision(node0,node1))
+ }//while(stack_collisions.size())
+ }
+public:
+ void find_collision(BOX_SET_CLASS0 * boxset1, const btTransform & trans1,
+ BOX_SET_CLASS1 * boxset2, const btTransform & trans2,
+ gim_pair_set & collision_pairs, bool complete_primitive_tests = true)
+ {
+ m_collision_pairs = &collision_pairs;
+ m_boxset0 = boxset1;
+ m_boxset1 = boxset2;
+
+ trans_cache_1to0.calc_from_homogenic(trans1,trans2);
+
+ trans_cache_0to1 = trans2.inverse();
+ trans_cache_0to1 *= trans1;
+
+
+ if(complete_primitive_tests)
+ {
+ t0_is_trimesh = boxset1->getPrimitiveManager().is_trimesh();
+ t1_is_trimesh = boxset2->getPrimitiveManager().is_trimesh();
+ }
+ else
+ {
+ t0_is_trimesh = false;
+ t1_is_trimesh = false;
+ }
+
+ find_collision_pairs();
+ }
+};
+
+
+#endif // GIM_BOXPRUNING_H_INCLUDED
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_clip_polygon.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_clip_polygon.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_clip_polygon.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_clip_polygon.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,210 @@
+#ifndef GIM_CLIP_POLYGON_H_INCLUDED
+#define GIM_CLIP_POLYGON_H_INCLUDED
+
+/*! \file gim_tri_collision.h
+\author Francisco Len Nßjera
+*/
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman at yahoo.com
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of EITHER:
+ (1) The GNU Lesser General Public License as published by the Free
+ Software Foundation; either version 2.1 of the License, or (at
+ your option) any later version. The text of the GNU Lesser
+ General Public License is included with this library in the
+ file GIMPACT-LICENSE-LGPL.TXT.
+ (2) The BSD-style license that is included with this library in
+ the file GIMPACT-LICENSE-BSD.TXT.
+ (3) The zlib/libpng license that is included with this library in
+ the file GIMPACT-LICENSE-ZLIB.TXT.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
+ GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+
+
+//! This function calcs the distance from a 3D plane
+class DISTANCE_PLANE_3D_FUNC
+{
+public:
+ template<typename CLASS_POINT,typename CLASS_PLANE>
+ inline GREAL operator()(const CLASS_PLANE & plane, const CLASS_POINT & point)
+ {
+ return DISTANCE_PLANE_POINT(plane, point);
+ }
+};
+
+
+
+template<typename CLASS_POINT>
+SIMD_FORCE_INLINE void PLANE_CLIP_POLYGON_COLLECT(
+ const CLASS_POINT & point0,
+ const CLASS_POINT & point1,
+ GREAL dist0,
+ GREAL dist1,
+ CLASS_POINT * clipped,
+ GUINT & clipped_count)
+{
+ GUINT _prevclassif = (dist0>G_EPSILON);
+ GUINT _classif = (dist1>G_EPSILON);
+ if(_classif!=_prevclassif)
+ {
+ GREAL blendfactor = -dist0/(dist1-dist0);
+ VEC_BLEND(clipped[clipped_count],point0,point1,blendfactor);
+ clipped_count++;
+ }
+ if(!_classif)
+ {
+ VEC_COPY(clipped[clipped_count],point1);
+ clipped_count++;
+ }
+}
+
+
+//! Clips a polygon by a plane
+/*!
+*\return The count of the clipped counts
+*/
+template<typename CLASS_POINT,typename CLASS_PLANE, typename DISTANCE_PLANE_FUNC>
+SIMD_FORCE_INLINE GUINT PLANE_CLIP_POLYGON_GENERIC(
+ const CLASS_PLANE & plane,
+ const CLASS_POINT * polygon_points,
+ GUINT polygon_point_count,
+ CLASS_POINT * clipped,DISTANCE_PLANE_FUNC distance_func)
+{
+ GUINT clipped_count = 0;
+
+
+ //clip first point
+ GREAL firstdist = distance_func(plane,polygon_points[0]);;
+ if(!(firstdist>G_EPSILON))
+ {
+ VEC_COPY(clipped[clipped_count],polygon_points[0]);
+ clipped_count++;
+ }
+
+ GREAL olddist = firstdist;
+ for(GUINT _i=1;_i<polygon_point_count;_i++)
+ {
+ GREAL dist = distance_func(plane,polygon_points[_i]);
+
+ PLANE_CLIP_POLYGON_COLLECT(
+ polygon_points[_i-1],polygon_points[_i],
+ olddist,
+ dist,
+ clipped,
+ clipped_count);
+
+
+ olddist = dist;
+ }
+
+ //RETURN TO FIRST point
+
+ PLANE_CLIP_POLYGON_COLLECT(
+ polygon_points[polygon_point_count-1],polygon_points[0],
+ olddist,
+ firstdist,
+ clipped,
+ clipped_count);
+
+ return clipped_count;
+}
+
+//! Clips a polygon by a plane
+/*!
+*\return The count of the clipped counts
+*/
+template<typename CLASS_POINT,typename CLASS_PLANE, typename DISTANCE_PLANE_FUNC>
+SIMD_FORCE_INLINE GUINT PLANE_CLIP_TRIANGLE_GENERIC(
+ const CLASS_PLANE & plane,
+ const CLASS_POINT & point0,
+ const CLASS_POINT & point1,
+ const CLASS_POINT & point2,
+ CLASS_POINT * clipped,DISTANCE_PLANE_FUNC distance_func)
+{
+ GUINT clipped_count = 0;
+
+ //clip first point
+ GREAL firstdist = distance_func(plane,point0);;
+ if(!(firstdist>G_EPSILON))
+ {
+ VEC_COPY(clipped[clipped_count],point0);
+ clipped_count++;
+ }
+
+ // point 1
+ GREAL olddist = firstdist;
+ GREAL dist = distance_func(plane,point1);
+
+ PLANE_CLIP_POLYGON_COLLECT(
+ point0,point1,
+ olddist,
+ dist,
+ clipped,
+ clipped_count);
+
+ olddist = dist;
+
+
+ // point 2
+ dist = distance_func(plane,point2);
+
+ PLANE_CLIP_POLYGON_COLLECT(
+ point1,point2,
+ olddist,
+ dist,
+ clipped,
+ clipped_count);
+ olddist = dist;
+
+
+
+ //RETURN TO FIRST point
+ PLANE_CLIP_POLYGON_COLLECT(
+ point2,point0,
+ olddist,
+ firstdist,
+ clipped,
+ clipped_count);
+
+ return clipped_count;
+}
+
+
+template<typename CLASS_POINT,typename CLASS_PLANE>
+SIMD_FORCE_INLINE GUINT PLANE_CLIP_POLYGON3D(
+ const CLASS_PLANE & plane,
+ const CLASS_POINT * polygon_points,
+ GUINT polygon_point_count,
+ CLASS_POINT * clipped)
+{
+ return PLANE_CLIP_POLYGON_GENERIC<CLASS_POINT,CLASS_PLANE>(plane,polygon_points,polygon_point_count,clipped,DISTANCE_PLANE_3D_FUNC());
+}
+
+
+template<typename CLASS_POINT,typename CLASS_PLANE>
+SIMD_FORCE_INLINE GUINT PLANE_CLIP_TRIANGLE3D(
+ const CLASS_PLANE & plane,
+ const CLASS_POINT & point0,
+ const CLASS_POINT & point1,
+ const CLASS_POINT & point2,
+ CLASS_POINT * clipped)
+{
+ return PLANE_CLIP_TRIANGLE_GENERIC<CLASS_POINT,CLASS_PLANE>(plane,point0,point1,point2,clipped,DISTANCE_PLANE_3D_FUNC());
+}
+
+
+
+#endif // GIM_TRI_COLLISION_H_INCLUDED
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_contact.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_contact.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_contact.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_contact.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,164 @@
+#ifndef GIM_CONTACT_H_INCLUDED
+#define GIM_CONTACT_H_INCLUDED
+
+/*! \file gim_contact.h
+\author Francisco Len Nßjera
+*/
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman at yahoo.com
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of EITHER:
+ (1) The GNU Lesser General Public License as published by the Free
+ Software Foundation; either version 2.1 of the License, or (at
+ your option) any later version. The text of the GNU Lesser
+ General Public License is included with this library in the
+ file GIMPACT-LICENSE-LGPL.TXT.
+ (2) The BSD-style license that is included with this library in
+ the file GIMPACT-LICENSE-BSD.TXT.
+ (3) The zlib/libpng license that is included with this library in
+ the file GIMPACT-LICENSE-ZLIB.TXT.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
+ GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+#include "gim_geometry.h"
+#include "gim_radixsort.h"
+#include "gim_array.h"
+
+
+/**
+Configuration var for applying interpolation of contact normals
+*/
+#define NORMAL_CONTACT_AVERAGE 1
+#define CONTACT_DIFF_EPSILON 0.00001f
+
+/// Structure for collision results
+///Functions for managing and sorting contacts resulting from a collision query.
+///Contact lists must be create by calling \ref GIM_CREATE_CONTACT_LIST
+///After querys, contact lists must be destroy by calling \ref GIM_DYNARRAY_DESTROY
+///Contacts can be merge for avoid duplicate results by calling \ref gim_merge_contacts
+class GIM_CONTACT
+{
+public:
+ btVector3 m_point;
+ btVector3 m_normal;
+ GREAL m_depth;//Positive value indicates interpenetration
+ GREAL m_distance;//Padding not for use
+ GUINT m_feature1;//Face number
+ GUINT m_feature2;//Face number
+public:
+ GIM_CONTACT()
+ {
+ }
+
+ GIM_CONTACT(const GIM_CONTACT & contact):
+ m_point(contact.m_point),
+ m_normal(contact.m_normal),
+ m_depth(contact.m_depth),
+ m_feature1(contact.m_feature1),
+ m_feature2(contact.m_feature2)
+ {
+ m_point = contact.m_point;
+ m_normal = contact.m_normal;
+ m_depth = contact.m_depth;
+ m_feature1 = contact.m_feature1;
+ m_feature2 = contact.m_feature2;
+ }
+
+ GIM_CONTACT(const btVector3 &point,const btVector3 & normal,
+ GREAL depth, GUINT feature1, GUINT feature2):
+ m_point(point),
+ m_normal(normal),
+ m_depth(depth),
+ m_feature1(feature1),
+ m_feature2(feature2)
+ {
+ }
+
+ //! Calcs key for coord classification
+ SIMD_FORCE_INLINE GUINT calc_key_contact() const
+ {
+ GINT _coords[] = {
+ (GINT)(m_point[0]*1000.0f+1.0f),
+ (GINT)(m_point[1]*1333.0f),
+ (GINT)(m_point[2]*2133.0f+3.0f)};
+ GUINT _hash=0;
+ GUINT *_uitmp = (GUINT *)(&_coords[0]);
+ _hash = *_uitmp;
+ _uitmp++;
+ _hash += (*_uitmp)<<4;
+ _uitmp++;
+ _hash += (*_uitmp)<<8;
+ return _hash;
+ }
+
+ SIMD_FORCE_INLINE void interpolate_normals( btVector3 * normals,GUINT normal_count)
+ {
+ btVector3 vec_sum(m_normal);
+ for(GUINT i=0;i<normal_count;i++)
+ {
+ vec_sum += normals[i];
+ }
+
+ GREAL vec_sum_len = vec_sum.length2();
+ if(vec_sum_len <CONTACT_DIFF_EPSILON) return;
+
+ GIM_INV_SQRT(vec_sum_len,vec_sum_len); // 1/sqrt(vec_sum_len)
+
+ m_normal = vec_sum*vec_sum_len;
+ }
+
+};
+
+
+class gim_contact_array:public gim_array<GIM_CONTACT>
+{
+public:
+ gim_contact_array():gim_array<GIM_CONTACT>(64)
+ {
+ }
+
+ SIMD_FORCE_INLINE void push_contact(const btVector3 &point,const btVector3 & normal,
+ GREAL depth, GUINT feature1, GUINT feature2)
+ {
+ push_back_mem();
+ GIM_CONTACT & newele = back();
+ newele.m_point = point;
+ newele.m_normal = normal;
+ newele.m_depth = depth;
+ newele.m_feature1 = feature1;
+ newele.m_feature2 = feature2;
+ }
+
+ SIMD_FORCE_INLINE void push_triangle_contacts(
+ const GIM_TRIANGLE_CONTACT_DATA & tricontact,
+ GUINT feature1,GUINT feature2)
+ {
+ for(GUINT i = 0;i<tricontact.m_point_count ;i++ )
+ {
+ push_back_mem();
+ GIM_CONTACT & newele = back();
+ newele.m_point = tricontact.m_points[i];
+ newele.m_normal = tricontact.m_separating_normal;
+ newele.m_depth = tricontact.m_penetration_depth;
+ newele.m_feature1 = feature1;
+ newele.m_feature2 = feature2;
+ }
+ }
+
+ void merge_contacts(const gim_contact_array & contacts, bool normal_contact_average = true);
+ void merge_contacts_unique(const gim_contact_array & contacts);
+};
+
+#endif // GIM_CONTACT_H_INCLUDED
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_geom_types.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_geom_types.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_geom_types.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_geom_types.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,97 @@
+#ifndef GIM_GEOM_TYPES_H_INCLUDED
+#define GIM_GEOM_TYPES_H_INCLUDED
+
+/*! \file gim_geom_types.h
+\author Francisco Len Nßjera
+*/
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman at yahoo.com
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of EITHER:
+ (1) The GNU Lesser General Public License as published by the Free
+ Software Foundation; either version 2.1 of the License, or (at
+ your option) any later version. The text of the GNU Lesser
+ General Public License is included with this library in the
+ file GIMPACT-LICENSE-LGPL.TXT.
+ (2) The BSD-style license that is included with this library in
+ the file GIMPACT-LICENSE-BSD.TXT.
+ (3) The zlib/libpng license that is included with this library in
+ the file GIMPACT-LICENSE-ZLIB.TXT.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
+ GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+
+
+#include "gim_math.h"
+
+
+
+//! Short Integer vector 2D
+typedef GSHORT vec2s[2];
+//! Integer vector 3D
+typedef GSHORT vec3s[3];
+//! Integer vector 4D
+typedef GSHORT vec4s[4];
+
+//! Short Integer vector 2D
+typedef GUSHORT vec2us[2];
+//! Integer vector 3D
+typedef GUSHORT vec3us[3];
+//! Integer vector 4D
+typedef GUSHORT vec4us[4];
+
+//! Integer vector 2D
+typedef GINT vec2i[2];
+//! Integer vector 3D
+typedef GINT vec3i[3];
+//! Integer vector 4D
+typedef GINT vec4i[4];
+
+//! Unsigned Integer vector 2D
+typedef GUINT vec2ui[2];
+//! Unsigned Integer vector 3D
+typedef GUINT vec3ui[3];
+//! Unsigned Integer vector 4D
+typedef GUINT vec4ui[4];
+
+//! Float vector 2D
+typedef GREAL vec2f[2];
+//! Float vector 3D
+typedef GREAL vec3f[3];
+//! Float vector 4D
+typedef GREAL vec4f[4];
+
+//! Double vector 2D
+typedef GREAL2 vec2d[2];
+//! Float vector 3D
+typedef GREAL2 vec3d[3];
+//! Float vector 4D
+typedef GREAL2 vec4d[4];
+
+//! Matrix 2D, row ordered
+typedef GREAL mat2f[2][2];
+//! Matrix 3D, row ordered
+typedef GREAL mat3f[3][3];
+//! Matrix 4D, row ordered
+typedef GREAL mat4f[4][4];
+
+//! Quaternion
+typedef GREAL quatf[4];
+
+//typedef struct _aabb3f aabb3f;
+
+
+
+#endif // GIM_GEOM_TYPES_H_INCLUDED
Added: test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_geometry.h
URL: http://llvm.org/viewvc/llvm-project/test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_geometry.h?rev=91782&view=auto
==============================================================================
--- test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_geometry.h (added)
+++ test-suite/trunk/MultiSource/Benchmarks/Bullet/include/BulletCollision/Gimpact/gim_geometry.h Sat Dec 19 14:05:59 2009
@@ -0,0 +1,42 @@
+#ifndef GIM_GEOMETRY_H_INCLUDED
+#define GIM_GEOMETRY_H_INCLUDED
+
+/*! \file gim_geometry.h
+\author Francisco Len Nßjera
+*/
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman at yahoo.com
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of EITHER:
+ (1) The GNU Lesser General Public License as published by the Free
+ Software Foundation; either version 2.1 of the License, or (at
+ your option) any later version. The text of the GNU Lesser
+ General Public License is included with this library in the
+ file GIMPACT-LICENSE-LGPL.TXT.
+ (2) The BSD-style license that is included with this library in
+ the file GIMPACT-LICENSE-BSD.TXT.
+ (3) The zlib/libpng license that is included with this library in
+ the file GIMPACT-LICENSE-ZLIB.TXT.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
+ GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+
+///Additional Headers for Collision
+#include "gim_basic_geometry_operations.h"
+#include "gim_clip_polygon.h"
+#include "gim_box_collision.h"
+#include "gim_tri_collision.h"
+
+#endif // GIM_VECTOR_H_INCLUDED
More information about the llvm-commits
mailing list