void btCreateLookAt(const btVector3& eye, const btVector3& center,const btVector3& up, float result[16])
{
btVector3 f = (center - eye).normalized();
btVector3 u = up.normalized();
btVector3 s = (f.cross(u)).normalized();
u = s.cross(f);
result[0*4+0] = s.x();
result[1*4+0] = s.y();
result[2*4+0] = s.z();
result[3*4+0] = -s.dot(eye);
result[0*4+1] = u.x();
result[1*4+1] = u.y();
result[2*4+1] = u.z();
result[3*4+1] = -u.dot(eye);
result[0*4+2] =-f.x();
result[1*4+2] =-f.y();
result[2*4+2] =-f.z();
result[3*4+2] = f.dot(eye);
result[0*4+3] = 0.f;
result[1*4+3] = 0.f;
result[2*4+3] = 0.f;
result[3*4+3] = 1.f;
}
// static helper method
static btVector3 getNormalizedVector(const btVector3& v)
{
btVector3 n = v.normalized();
if (n.length() < SIMD_EPSILON) {
n.setValue(0, 0, 0);
}
return n;
}
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.)) );
}
void btGeneric6DofSpringConstraint::setAxis(const btVector3& axis1, const btVector3& axis2)
{
btVector3 zAxis = axis1.normalized();
btVector3 yAxis = axis2.normalized();
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]);
// now get constraint frame in local coordinate systems
m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW;
m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW;
calculateTransforms();
}
// static helper method
static btVector3
getNormalizedVector(const btVector3& v)
{
btVector3 n(0, 0, 0);
if (v.length() > SIMD_EPSILON) {
n = v.normalized();
}
return n;
}
PhysicsSystem::PhysicsSystem(Engine& engine, const float& gravity, const btVector3& rotation) :
System(engine),
collisionConfiguration(*new btDefaultCollisionConfiguration()),
dispatcher(*new btCollisionDispatcher(&collisionConfiguration)),
broadphase(*new btDbvtBroadphase()),
solver(*new btSequentialImpulseConstraintSolver),
dynamicsWorld(*new btDiscreteDynamicsWorld(&dispatcher, &broadphase, &solver, &collisionConfiguration)),
gravity(gravity),
rotation(rotation.normalized())
{
dynamicsWorld.setGravity(gravity*rotation);
} //PhysicsSystem
bool BtPlayer::_sweep( btVector3 *inOutCurrPos, const btVector3 &disp, CollisionList *outCol )
{
btTransform start( btTransform::getIdentity() );
start.setOrigin ( *inOutCurrPos );
btTransform end( btTransform::getIdentity() );
end.setOrigin ( *inOutCurrPos + disp );
BtPlayerSweepCallback callback( mGhostObject, disp.normalized() );
callback.m_collisionFilterGroup = mGhostObject->getBroadphaseHandle()->m_collisionFilterGroup;
callback.m_collisionFilterMask = mGhostObject->getBroadphaseHandle()->m_collisionFilterMask;
mGhostObject->convexSweepTest( mColShape, start, end, callback, 0.0f );
inOutCurrPos->setInterpolate3( start.getOrigin(), end.getOrigin(), callback.m_closestHitFraction );
if ( callback.hasHit() )
{
if ( outCol )
{
Collision& col = outCol->increment();
dMemset( &col, 0, sizeof( col ) );
col.normal = btCast<Point3F>( callback.m_hitNormalWorld );
col.object = PhysicsUserData::getObject( callback.m_hitCollisionObject->getUserPointer() );
if (disp.z() < 0.0f)
{
// We're sweeping down as part of the stepping routine. In this
// case we want to have the collision normal only point in the opposite direction.
// i.e. up If we include the sideways part of the normal then the Player class
// velocity calculations using this normal will affect the player's forwards
// momentum. This is especially noticable on stairs as the rounded bottom of
// the capsule slides up the corner of a stair.
col.normal.set(0.0f, 0.0f, 1.0f);
}
}
return true;
}
return false;
}
void btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatingNormal1, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut)
{
btVector3 separatingNormal = separatingNormal1.normalized();
const btVector3 c0 = transA * hullA.m_localCenter;
const btVector3 c1 = transB * hullB.m_localCenter;
const btVector3 DeltaC2 = c0 - c1;
btScalar curMaxDist=maxDist;
int closestFaceB=-1;
btScalar dmax = -FLT_MAX;
{
for(int face=0;face<hullB.m_faces.size();face++)
{
const btVector3 Normal(hullB.m_faces[face].m_plane[0], hullB.m_faces[face].m_plane[1], hullB.m_faces[face].m_plane[2]);
const btVector3 WorldNormal = transB.getBasis() * Normal;
btScalar d = WorldNormal.dot(separatingNormal);
if (d > dmax)
{
dmax = d;
closestFaceB = face;
}
}
}
btVertexArray worldVertsB1;
{
const btFace& polyB = hullB.m_faces[closestFaceB];
const int numVertices = polyB.m_indices.size();
for(int e0=0;e0<numVertices;e0++)
{
const btVector3& b = hullB.m_vertices[polyB.m_indices[e0]];
worldVertsB1.push_back(transB*b);
}
}
if (closestFaceB>=0)
clipFaceAgainstHull(separatingNormal, hullA, transA,worldVertsB1, minDist, maxDist,resultOut);
}
// static helper method
static btVector3
getNormalizedVector(const btVector3& v)
{
/*btVector3 n(0, 0, 0);
if (v.fuzzyZero()) return n;
if (v.length() > SIMD_EPSILON) {
n = v.normalized();
}
return n;*/
btVector3 n;
if (v.fuzzyZero()) {
n.setValue(0, 0, 0);
}
else {
n = v.normalized();
}
return n;
}
static inline btVector3 Vector3Rand()
{
const btVector3 p=btVector3(SignedUnitRand(),SignedUnitRand(),SignedUnitRand());
return(p.normalized());
}
//-----------------------------------------------------------------------
// s t e p F o r w a r d A n d S t r a f e
//-----------------------------------------------------------------------
void TKinematicCharacterTest::stepForwardAndStrafe (btCollisionWorld* collisionWorld, const btVector3& walkMove)
{
btVector3 originalDir = walkMove.normalized();
if (walkMove.length() < SIMD_EPSILON)
{
originalDir.setValue(0.f,0.f,0.f);
}
// printf("originalDir=%f,%f,%f\n",originalDir[0],originalDir[1],originalDir[2]);
// phase 2: forward and strafe
btTransform start, end;
m_targetPosition = m_currentPosition + walkMove;
start.setIdentity ();
end.setIdentity ();
btScalar fraction = 1.0;
btScalar distance2 = (m_currentPosition-m_targetPosition).length2();
// printf("distance2=%f\n",distance2);
if (m_touchingContact)
{
if (originalDir.dot(m_touchingNormal) > btScalar(0.0))
updateTargetPositionBasedOnCollision (m_touchingNormal);
}
int maxIter = 10;
while (fraction > btScalar(0.01) && maxIter-- > 0)
{
start.setOrigin (m_currentPosition);
end.setOrigin (m_targetPosition);
TKinematicClosestNotMeConvexResultCallback callback (m_ghostObject);
callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
btScalar margin = m_convexShape->getMargin();
m_convexShape->setMargin(margin + m_addedMargin);
if (m_useGhostObjectSweepTest)
{
m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
} else
{
collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
}
m_convexShape->setMargin(margin);
fraction -= callback.m_closestHitFraction;
if (callback.hasHit())
{
if(callback.m_hitCollisionObject->isStaticObject())
{
// we moved only a fraction
btScalar hitDistance = (callback.m_hitPointWorld - m_currentPosition).length();
if (hitDistance<0.f)
{
// printf("neg dist?\n");
}
/* If the distance is farther than the collision margin, move */
if (hitDistance > m_addedMargin)
{
// printf("callback.m_closestHitFraction=%f\n",callback.m_closestHitFraction);
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
}
updateTargetPositionBasedOnCollision (callback.m_hitNormalWorld);
btVector3 currentDir = m_targetPosition - m_currentPosition;
distance2 = currentDir.length2();
if (distance2 > SIMD_EPSILON)
{
currentDir.normalize();
/* See Quake2: "If velocity is against original velocity, stop ead to avoid tiny oscilations in sloping corners." */
if (currentDir.dot(originalDir) <= btScalar(0.0))
{
break;
}
} else
{
// printf("currentDir: don't normalize a zero vector\n");
break;
}
}
} else {
// we moved whole way
m_currentPosition = m_targetPosition;
}
// if (callback.m_closestHitFraction == 0.f)
// break;
}
}