//#include "stdio.h"
void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
{
BT_PROFILE("integrateTransforms");
btTransform predictedTrans;
for ( int i=0;i<m_collisionObjects.size();i++)
{
btCollisionObject* colObj = m_collisionObjects[i];
btRigidBody* body = btRigidBody::upcast(colObj);
if (body)
{
body->setHitFraction(1.f);
if (body->isActive() && (!body->isStaticOrKinematicObject()))
{
body->predictIntegratedTransform(timeStep, predictedTrans);
btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();
if (body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)
{
BT_PROFILE("CCD motion clamping");
if (body->getCollisionShape()->isConvex())
{
gNumClampedCcdMotions++;
btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup;
sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask;
convexSweepTest(&tmpSphere,body->getWorldTransform(),predictedTrans,sweepResults);
if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))
{
body->setHitFraction(sweepResults.m_closestHitFraction);
body->predictIntegratedTransform(timeStep*body->getHitFraction(), predictedTrans);
body->setHitFraction(0.f);
// printf("clamped integration to hit fraction = %f\n",fraction);
}
}
}
body->proceedToTransform( predictedTrans);
}
}
}
}
//-------------------------------------------------------------
VCNResID VCNMeshLoader::LoadMeshElementPositionXML( XMLNodePtr elementNode, VCNSphere* bounding, VCNAabb* aabb /*= NULL*/ )
{
// Fetch the node we need from the element node
XMLNodePtr node = 0;
elementNode->selectSingleNode( (VCNTChar*)kNodeVertexPositions, &node );
VCN_ASSERT( node != NULL && "No positions in mesh!" );
// Get the expected size of the array
VCNUInt size = 0;
GetAttributeUInt( node, kAttrVertexPositionsSize, size );
// If we don't have any, leave.
if( size == 0 )
return kInvalidResID;
// Create an array to contain all of this (3 floats per position)
VCNUInt stride = size * kPositionFloats;
VCNFloat* buffer = new VCNFloat[ stride ];
// Create some tools...
VCNFloat* ptrFloat = buffer;
VCNInt safety = 0;
// Keep track of the min and max
VCNFloat minX, maxX;
VCNFloat minY, maxY;
VCNFloat minZ, maxZ;
minX = minY = minZ = kMaxFloat;
maxX = maxY = maxZ = kMinFloat;
// Read the XML and fill the array!
XMLNodeListPtr positions = 0;
node->selectNodes( (VCNTChar*)kNodeVertexPosition, &positions );
VCN_ASSERT( positions != 0 && "FILE IS CORRUPTED!" );
VCNLong positionsLength = 0;
positions->get_length( &positionsLength );
VCN_ASSERT( positionsLength == size && "FILE IS CORRUPTED!" );
for( VCNLong i=0; i<positionsLength; i++ )
{
// Get the element's node
XMLNodePtr positionNode = 0;
positions->get_item( i, &positionNode );
// Read the X
GetAttributeFloat( positionNode, kAttrVertexPositionX, *ptrFloat );
if( *ptrFloat < minX )
minX = *ptrFloat;
if( *ptrFloat > maxX )
maxX = *ptrFloat;
ptrFloat++;
// Read the Y
GetAttributeFloat( positionNode, kAttrVertexPositionY, *ptrFloat );
if( *ptrFloat < minY )
minY = *ptrFloat;
if( *ptrFloat > maxY )
maxY = *ptrFloat;
ptrFloat++;
// Read the Z
GetAttributeFloat( positionNode, kAttrVertexPositionZ, *ptrFloat );
if( *ptrFloat < minZ )
minZ = *ptrFloat;
if( *ptrFloat > maxZ )
maxZ = *ptrFloat;
ptrFloat++;
// Verify the safety to make sure we're reading in the right order
GetAttributeInt( positionNode, kAttrVertexPositionID, safety );
VCN_ASSERT( safety==i && "VERTEX AREN'T READ IN ORDER!" );
}
// Now give the information to the cache manager
// (he'll take care of making this data API specific)
VCNResID cacheID = VCNRenderCore::GetInstance()->CreateCache( VT_POSITION, buffer, stride*sizeof(VCNFloat) );
// Clear the buffer
delete [] buffer;
Vector3 minVect ( minX, minY, minZ );
Vector3 maxVect ( maxX, maxY, maxZ );
Vector3 diagonal = (maxVect - minVect) / 2.0f;
// If he wants us to fill the AABB, we'll do it for him
if( bounding )
{
VCNSphere tmpSphere( diagonal.Length(), minVect + diagonal );
*bounding = tmpSphere;
}
if (aabb)
{
VCNAabb tempAabb(minVect, maxVect);
*aabb = tempAabb;
}
// Return the cache ID
return cacheID;
}
void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
{
BT_PROFILE("integrateTransforms");
btTransform predictedTrans;
for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
{
btRigidBody* body = m_nonStaticRigidBodies[i];
body->setHitFraction(1.f);
if (body->isActive() && (!body->isStaticOrKinematicObject()))
{
body->predictIntegratedTransform(timeStep, predictedTrans);
btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();
if (getDispatchInfo().m_useContinuous && body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)
{
BT_PROFILE("CCD motion clamping");
if (body->getCollisionShape()->isConvex())
{
gNumClampedCcdMotions++;
#ifdef USE_STATIC_ONLY
class StaticOnlyCallback : public btClosestNotMeConvexResultCallback
{
public:
StaticOnlyCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :
btClosestNotMeConvexResultCallback(me,fromA,toA,pairCache,dispatcher)
{
}
virtual bool needsCollision(btBroadphaseProxy* proxy0) const
{
btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject;
if (!otherObj->isStaticOrKinematicObject())
return false;
return btClosestNotMeConvexResultCallback::needsCollision(proxy0);
}
};
StaticOnlyCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
#else
btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
#endif
//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
sweepResults.m_allowedPenetration=getDispatchInfo().m_allowedCcdPenetration;
sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup;
sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask;
btTransform modifiedPredictedTrans = predictedTrans;
modifiedPredictedTrans.setBasis(body->getWorldTransform().getBasis());
convexSweepTest(&tmpSphere,body->getWorldTransform(),modifiedPredictedTrans,sweepResults);
if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))
{
//printf("clamped integration to hit fraction = %f\n",fraction);
body->setHitFraction(sweepResults.m_closestHitFraction);
body->predictIntegratedTransform(timeStep*body->getHitFraction(), predictedTrans);
body->setHitFraction(0.f);
body->proceedToTransform( predictedTrans);
#if 0
btVector3 linVel = body->getLinearVelocity();
btScalar maxSpeed = body->getCcdMotionThreshold()/getSolverInfo().m_timeStep;
btScalar maxSpeedSqr = maxSpeed*maxSpeed;
if (linVel.length2()>maxSpeedSqr)
{
linVel.normalize();
linVel*= maxSpeed;
body->setLinearVelocity(linVel);
btScalar ms2 = body->getLinearVelocity().length2();
body->predictIntegratedTransform(timeStep, predictedTrans);
btScalar sm2 = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();
btScalar smt = body->getCcdSquareMotionThreshold();
printf("sm2=%f\n",sm2);
}
#else
//don't apply the collision response right now, it will happen next frame
//if you really need to, you can uncomment next 3 lines. Note that is uses zero restitution.
//btScalar appliedImpulse = 0.f;
//btScalar depth = 0.f;
//appliedImpulse = resolveSingleCollision(body,(btCollisionObject*)sweepResults.m_hitCollisionObject,sweepResults.m_hitPointWorld,sweepResults.m_hitNormalWorld,getSolverInfo(), depth);
#endif
continue;
}
}
}
body->proceedToTransform( predictedTrans);
}
}
///this should probably be switched on by default, but it is not well tested yet
if (m_applySpeculativeContactRestitution)
{
BT_PROFILE("apply speculative contact restitution");
for (int i=0;i<m_predictiveManifolds.size();i++)
{
btPersistentManifold* manifold = m_predictiveManifolds[i];
btRigidBody* body0 = btRigidBody::upcast((btCollisionObject*)manifold->getBody0());
btRigidBody* body1 = btRigidBody::upcast((btCollisionObject*)manifold->getBody1());
for (int p=0;p<manifold->getNumContacts();p++)
{
const btManifoldPoint& pt = manifold->getContactPoint(p);
btScalar combinedRestitution = btManifoldResult::calculateCombinedRestitution(body0, body1);
if (combinedRestitution>0 && pt.m_appliedImpulse != 0.f)
//if (pt.getDistance()>0 && combinedRestitution>0 && pt.m_appliedImpulse != 0.f)
{
btVector3 imp = -pt.m_normalWorldOnB * pt.m_appliedImpulse* combinedRestitution;
const btVector3& pos1 = pt.getPositionWorldOnA();
const btVector3& pos2 = pt.getPositionWorldOnB();
btVector3 rel_pos0 = pos1 - body0->getWorldTransform().getOrigin();
btVector3 rel_pos1 = pos2 - body1->getWorldTransform().getOrigin();
if (body0)
body0->applyImpulse(imp,rel_pos0);
if (body1)
body1->applyImpulse(-imp,rel_pos1);
}
}
}
}
}
void btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep)
{
BT_PROFILE("createPredictiveContacts");
{
BT_PROFILE("release predictive contact manifolds");
for (int i=0;i<m_predictiveManifolds.size();i++)
{
btPersistentManifold* manifold = m_predictiveManifolds[i];
this->m_dispatcher1->releaseManifold(manifold);
}
m_predictiveManifolds.clear();
}
btTransform predictedTrans;
for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
{
btRigidBody* body = m_nonStaticRigidBodies[i];
body->setHitFraction(1.f);
if (body->isActive() && (!body->isStaticOrKinematicObject()))
{
body->predictIntegratedTransform(timeStep, predictedTrans);
btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();
if (getDispatchInfo().m_useContinuous && body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)
{
BT_PROFILE("predictive convexSweepTest");
if (body->getCollisionShape()->isConvex())
{
gNumClampedCcdMotions++;
#ifdef PREDICTIVE_CONTACT_USE_STATIC_ONLY
class StaticOnlyCallback : public btClosestNotMeConvexResultCallback
{
public:
StaticOnlyCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :
btClosestNotMeConvexResultCallback(me,fromA,toA,pairCache,dispatcher)
{
}
virtual bool needsCollision(btBroadphaseProxy* proxy0) const
{
btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject;
if (!otherObj->isStaticOrKinematicObject())
return false;
return btClosestNotMeConvexResultCallback::needsCollision(proxy0);
}
};
StaticOnlyCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
#else
btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
#endif
//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
sweepResults.m_allowedPenetration=getDispatchInfo().m_allowedCcdPenetration;
sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup;
sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask;
btTransform modifiedPredictedTrans = predictedTrans;
modifiedPredictedTrans.setBasis(body->getWorldTransform().getBasis());
convexSweepTest(&tmpSphere,body->getWorldTransform(),modifiedPredictedTrans,sweepResults);
if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))
{
btVector3 distVec = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin())*sweepResults.m_closestHitFraction;
btScalar distance = distVec.dot(-sweepResults.m_hitNormalWorld);
btPersistentManifold* manifold = m_dispatcher1->getNewManifold(body,sweepResults.m_hitCollisionObject);
m_predictiveManifolds.push_back(manifold);
btVector3 worldPointB = body->getWorldTransform().getOrigin()+distVec;
btVector3 localPointB = sweepResults.m_hitCollisionObject->getWorldTransform().inverse()*worldPointB;
btManifoldPoint newPoint(btVector3(0,0,0), localPointB,sweepResults.m_hitNormalWorld,distance);
bool isPredictive = true;
int index = manifold->addManifoldPoint(newPoint, isPredictive);
btManifoldPoint& pt = manifold->getContactPoint(index);
pt.m_combinedRestitution = 0;
pt.m_combinedFriction = btManifoldResult::calculateCombinedFriction(body,sweepResults.m_hitCollisionObject);
pt.m_positionWorldOnA = body->getWorldTransform().getOrigin();
pt.m_positionWorldOnB = worldPointB;
}
}
}
}
}
}
void btDiscreteDynamicsWorld::addSpeculativeContacts(btScalar timeStep)
{
BT_PROFILE("addSpeculativeContacts");
btTransform predictedTrans;
for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
{
btRigidBody* body = m_nonStaticRigidBodies[i];
body->setHitFraction(1.f);
if (body->isActive() && (!body->isStaticOrKinematicObject()))
{
body->predictIntegratedTransform(timeStep, predictedTrans);
btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();
if (body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)
{
BT_PROFILE("search speculative contacts");
if (body->getCollisionShape()->isConvex())
{
gNumClampedCcdMotions++;
btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup;
sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask;
btTransform modifiedPredictedTrans;
modifiedPredictedTrans = predictedTrans;
modifiedPredictedTrans.setBasis(body->getWorldTransform().getBasis());
convexSweepTest(&tmpSphere,body->getWorldTransform(),modifiedPredictedTrans,sweepResults);
if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))
{
btBroadphaseProxy* proxy0 = body->getBroadphaseHandle();
btBroadphaseProxy* proxy1 = sweepResults.m_hitCollisionObject->getBroadphaseHandle();
btBroadphasePair* pair = sweepResults.m_pairCache->findPair(proxy0,proxy1);
if (pair)
{
if (pair->m_algorithm)
{
btManifoldArray contacts;
pair->m_algorithm->getAllContactManifolds(contacts);
if (contacts.size())
{
btManifoldResult result(body,sweepResults.m_hitCollisionObject);
result.setPersistentManifold(contacts[0]);
btVector3 vec = (modifiedPredictedTrans.getOrigin()-body->getWorldTransform().getOrigin());
vec*=sweepResults.m_closestHitFraction;
btScalar lenSqr = vec.length2();
btScalar depth = 0.f;
btVector3 pointWorld = sweepResults.m_hitPointWorld;
if (lenSqr>SIMD_EPSILON)
{
depth = btSqrt(lenSqr);
pointWorld -= vec;
vec /= depth;
}
if (contacts[0]->getBody0()==body)
{
result.addContactPoint(sweepResults.m_hitNormalWorld,pointWorld,depth);
#if 0
debugContacts.push_back(sweepResults.m_hitPointWorld);//sweepResults.m_hitPointWorld);
debugNormals.push_back(sweepResults.m_hitNormalWorld);
#endif
} else
{
//swapped
result.addContactPoint(-sweepResults.m_hitNormalWorld,pointWorld,depth);
//sweepResults.m_hitPointWorld,depth);
#if 0
if (1)//firstHit==1)
{
firstHit=0;
debugNormals.push_back(sweepResults.m_hitNormalWorld);
debugContacts.push_back(pointWorld);//sweepResults.m_hitPointWorld);
debugNormals.push_back(sweepResults.m_hitNormalWorld);
debugContacts.push_back(sweepResults.m_hitPointWorld);
}
firstHit--;
#endif
}
}
} else
{
//no algorithm, use dispatcher to create one
}
} else
{
//add an overlapping pair
//printf("pair missing\n");
}
}
}
}
}
}
}
void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
{
BT_PROFILE("integrateTransforms");
btTransform predictedTrans;
for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
{
btRigidBody* body = m_nonStaticRigidBodies[i];
body->setHitFraction(1.f);
if (body->isActive() && (!body->isStaticOrKinematicObject()))
{
body->predictIntegratedTransform(timeStep, predictedTrans);
btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();
if (getDispatchInfo().m_useContinuous && body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)
{
BT_PROFILE("CCD motion clamping");
if (body->getCollisionShape()->isConvex())
{
gNumClampedCcdMotions++;
#ifdef USE_STATIC_ONLY
class StaticOnlyCallback : public btClosestNotMeConvexResultCallback
{
public:
StaticOnlyCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :
btClosestNotMeConvexResultCallback(me,fromA,toA,pairCache,dispatcher)
{
}
virtual bool needsCollision(btBroadphaseProxy* proxy0) const
{
btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject;
if (!otherObj->isStaticOrKinematicObject())
return false;
return btClosestNotMeConvexResultCallback::needsCollision(proxy0);
}
};
StaticOnlyCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
#else
btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
#endif
//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
sweepResults.m_allowedPenetration=getDispatchInfo().m_allowedCcdPenetration;
sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup;
sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask;
btTransform modifiedPredictedTrans = predictedTrans;
modifiedPredictedTrans.setBasis(body->getWorldTransform().getBasis());
convexSweepTest(&tmpSphere,body->getWorldTransform(),modifiedPredictedTrans,sweepResults);
if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))
{
//printf("clamped integration to hit fraction = %f\n",fraction);
body->setHitFraction(sweepResults.m_closestHitFraction);
body->predictIntegratedTransform(timeStep*body->getHitFraction(), predictedTrans);
body->setHitFraction(0.f);
body->proceedToTransform( predictedTrans);
#if 0
btVector3 linVel = body->getLinearVelocity();
btScalar maxSpeed = body->getCcdMotionThreshold()/getSolverInfo().m_timeStep;
btScalar maxSpeedSqr = maxSpeed*maxSpeed;
if (linVel.length2()>maxSpeedSqr)
{
linVel.normalize();
linVel*= maxSpeed;
body->setLinearVelocity(linVel);
btScalar ms2 = body->getLinearVelocity().length2();
body->predictIntegratedTransform(timeStep, predictedTrans);
btScalar sm2 = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();
btScalar smt = body->getCcdSquareMotionThreshold();
printf("sm2=%f\n",sm2);
}
#else
//response between two dynamic objects without friction, assuming 0 penetration depth
btScalar appliedImpulse = 0.f;
btScalar depth = 0.f;
appliedImpulse = resolveSingleCollision(body,sweepResults.m_hitCollisionObject,sweepResults.m_hitPointWorld,sweepResults.m_hitNormalWorld,getSolverInfo(), depth);
#endif
continue;
}
}
}
body->proceedToTransform( predictedTrans);
}
}
}
