void clientMouseFunc(int button, int state, int x, int y)
{
//printf("button %i, state %i, x=%i,y=%i\n",button,state,x,y);
//button 0, state 0 means left mouse down
SimdVector3 rayTo = GetRayTo(x,y);
switch (button)
{
case 2:
{
if (state==0)
{
shootBox(rayTo);
}
break;
};
case 1:
{
if (state==0)
{
//apply an impulse
if (physicsEnvironmentPtr)
{
float hit[3];
float normal[3];
PHY_IPhysicsController* hitObj = physicsEnvironmentPtr->rayTest(0,eye[0],eye[1],eye[2],rayTo.getX(),rayTo.getY(),rayTo.getZ(),hit[0],hit[1],hit[2],normal[0],normal[1],normal[2]);
if (hitObj)
{
CcdPhysicsController* physCtrl = static_cast<CcdPhysicsController*>(hitObj);
RigidBody* body = physCtrl->GetRigidBody();
if (body)
{
body->SetActivationState(ACTIVE_TAG);
SimdVector3 impulse = rayTo;
impulse.normalize();
float impulseStrength = 10.f;
impulse *= impulseStrength;
SimdVector3 relPos(
hit[0] - body->getCenterOfMassPosition().getX(),
hit[1] - body->getCenterOfMassPosition().getY(),
hit[2] - body->getCenterOfMassPosition().getZ());
body->applyImpulse(impulse,relPos);
}
}
}
} else
{
}
break;
}
case 0:
{
if (state==0)
{
//add a point to point constraint for picking
if (physicsEnvironmentPtr)
{
float hit[3];
float normal[3];
PHY_IPhysicsController* hitObj = physicsEnvironmentPtr->rayTest(0,eye[0],eye[1],eye[2],rayTo.getX(),rayTo.getY(),rayTo.getZ(),hit[0],hit[1],hit[2],normal[0],normal[1],normal[2]);
if (hitObj)
{
CcdPhysicsController* physCtrl = static_cast<CcdPhysicsController*>(hitObj);
RigidBody* body = physCtrl->GetRigidBody();
if (body)
{
pickedBody = body;
pickedBody->SetActivationState(DISABLE_DEACTIVATION);
SimdVector3 pickPos(hit[0],hit[1],hit[2]);
SimdVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos;
gPickingConstraintId = physicsEnvironmentPtr->createConstraint(physCtrl,0,PHY_POINT2POINT_CONSTRAINT,
localPivot.getX(),
localPivot.getY(),
localPivot.getZ(),
0,0,0);
//printf("created constraint %i",gPickingConstraintId);
//save mouse position for dragging
gOldPickingPos = rayTo;
SimdVector3 eyePos(eye[0],eye[1],eye[2]);
gOldPickingDist = (pickPos-eyePos).length();
Point2PointConstraint* p2p = static_cast<Point2PointConstraint*>(physicsEnvironmentPtr->getConstraintById(gPickingConstraintId));
if (p2p)
{
//very weak constraint for picking
p2p->m_setting.m_tau = 0.1f;
}
}
}
}
} else
{
if (gPickingConstraintId && physicsEnvironmentPtr)
{
physicsEnvironmentPtr->removeConstraint(gPickingConstraintId);
//printf("removed constraint %i",gPickingConstraintId);
gPickingConstraintId = 0;
pickedBody->ForceActivationState(ACTIVE_TAG);
pickedBody->m_deactivationTime = 0.f;
pickedBody = 0;
}
}
break;
}
default:
{
}
}
}
void CollisionSolver::prep(float dt, const Vector<ContactManifold*>& manifolds)
{
float idt = dt == 0.0f ? 0.0f : 1.0f / dt;
for (int h = -1; ++h < manifolds.size();)
{
ContactManifold* manifold = manifolds[h];
CollisionObject* a = manifold->getA();
CollisionObject* b = manifold->getB();
RigidBody* ra = a->getBody();
RigidBody* rb = b->getBody();
for (int i = -1; ++i < manifold->numContacts();)
{
Contact *c = manifold->getContact(i);
if (!c) continue;
float ida = ra ? ra->getImpulseDenominator(c->mRcp1, c->mNormal) : 0.f;
float idb = rb ? rb->getImpulseDenominator(c->mRcp2, c->mNormal) : 0.f;
c->mMassNormal = 1.0f / (ida + idb);
Vector3 vela = ra ? ra->getVelocity(c->mRcp1) : Vector3::ZERO;
Vector3 velb = rb ? rb->getVelocity(c->mRcp2) : Vector3::ZERO;
Vector3 rvel = velb - vela;
float vn = rvel.dot(c->mNormal);
Vector3 tangent = c->mNormal * vn;
tangent = rvel - tangent;
Vector3 ttemp = tangent;
tangent = ttemp - (ttemp.dot(c->mNormal) * c->mNormal);
if (tangent != Vector3::ZERO)
{
tangent.normalise();
float idta = ra ? ra->getImpulseDenominator(c->mRcp1, tangent) : 0.f;
float idtb = rb ? rb->getImpulseDenominator(c->mRcp2, tangent) : 0.f;
float mtangent = idta + idtb;
c->mMassTangent = 1.0f / mtangent;
c->mTangent = tangent;
}
else
{
c->mMassTangent = 0.0f;
c->mTangent = Vector3::ZERO;
}
c->mBias = -mSimulator->getCollisionBiasFactor() * idt * Math::min(0.0f, -c->mPenetration + mSimulator->getAllowedPenetration());
float veln = -rvel.dot(c->mNormal);
float resta = ra ? ra->getRestitution() : 1.0f;
float restb = rb ? rb->getRestitution() : 1.0f;
float restitution = Math::min(resta, restb);
c->mDV = veln > (c->mMassNormal * mSimulator->getRestingVelocityLimit() * 2.0f) ? (1.0f + restitution) * (c->mMassNormal * veln) : 0.0f; //TODO: Constant here, make it configurable?
Vector3 impulse = (c->mPN * c->mNormal) + (c->mPT * c->mTangent);
if (ra) ra->applyImpulse(-impulse, c->mRcp1);
if (rb) rb->applyImpulse(impulse, c->mRcp2);
}
}
}
void CollisionSolver::step(const Vector<ContactManifold*>& manifolds)
{
for (int h = -1; ++h < manifolds.size();)
{
ContactManifold* manifold = manifolds[h];
CollisionObject* a = manifold->getA();
CollisionObject* b = manifold->getB();
RigidBody* ra = a->getBody();
RigidBody* rb = b->getBody();
for (int i = -1; ++i < manifold->numContacts();)
{
Contact *c = manifold->getContact(i);
if (!c) continue;
Vector3 vela = ra ? ra->getVelocity(c->mRcp1) : Vector3::ZERO;
Vector3 velb = rb ? rb->getVelocity(c->mRcp2) : Vector3::ZERO;
Vector3 rvel = velb - vela;
float veln = rvel.dot(c->mNormal);
float deltaImpulse = c->mMassNormal * -veln;
float pn0 = c->mPN;
c->mPN = Math::max(pn0 + deltaImpulse, c->mDV);
deltaImpulse = c->mPN - pn0;
Vector3 impulse = c->mNormal * deltaImpulse;
if (ra) ra->applyImpulse(-impulse, c->mRcp1);
if (rb) rb->applyImpulse(impulse, c->mRcp2);
vela = ra ? ra->getBiasVelocity(c->mRcp1) : Vector3::ZERO;
velb = rb ? rb->getBiasVelocity(c->mRcp2) : Vector3::ZERO;
rvel = velb - vela;
float velnb = rvel.dot(c->mNormal);
float dPnb = c->mMassNormal * (-velnb + c->mBias);
float pnb0 = c->mPNB;
c->mPNB = Math::max(pnb0 + dPnb, c->mDV);
dPnb = c->mPNB - pnb0;
Vector3 impulseb = c->mNormal * dPnb;
if (ra) ra->applyBiasImpulse(-impulseb, c->mRcp1);
if (rb) rb->applyBiasImpulse(impulseb, c->mRcp2);
if (c->mTangent != Vector3::ZERO)
{
vela = ra ? ra->getVelocity(c->mRcp1) : Vector3::ZERO;
velb = rb ? rb->getVelocity(c->mRcp2) : Vector3::ZERO;
rvel = velb - vela;
float velt = c->mTangent.dot(rvel);
float deltaImpulseTangent = c->mMassTangent * -velt;
float fa = ra ? ra->getFriction() : 0.0f;
float fb = rb ? rb->getFriction() : 0.0f;
float maxPt = Math::max(fa, fb) * c->mPN;
float oldTangentImpulse = c->mPT;
c->mPT = Math::clamp(oldTangentImpulse + deltaImpulseTangent, -maxPt, maxPt);
deltaImpulseTangent = c->mPT - oldTangentImpulse;
Vector3 frictionImpulse = c->mTangent * deltaImpulseTangent;
if (ra) ra->applyImpulse(-frictionImpulse, c->mRcp1);
if (rb) rb->applyImpulse(frictionImpulse, c->mRcp2);
}
}
}
}
void World::simulate(float dt)
{
mAllocator.clearFrame();
int vIterNum = 50;
int pIterNum = 10;
mColManager.preocss( dt );
for( RigidBodyList::iterator iter = mRigidBodies.begin() ,itEnd = mRigidBodies.end();
iter != itEnd ; ++iter )
{
RigidBody* body = *iter;
body->saveState();
if ( body->getMotionType() != BodyMotion::eStatic )
{
if ( body->getMotionType() == BodyMotion::eDynamic )
body->addLinearImpulse( body->mMass * mGrivaty * dt );
body->applyImpulse();
body->mLinearVel *= 1.0 / ( 1 + body->mLinearDamping );
}
}
ContactManifold** sortedContact = new ( mAllocator ) ContactManifold* [ mColManager.mMainifolds.size() ];
int numMainfold = mColManager.mMainifolds.size();
int idxStatic = mColManager.mMainifolds.size() - 1;
int idxNormal = 0;
for( int i = 0 ; i < mColManager.mMainifolds.size() ; ++i )
{
ContactManifold& cm = *mColManager.mMainifolds[i];
RigidBody* bodyA = static_cast< RigidBody* >( cm.mContect.object[0] );
RigidBody* bodyB = static_cast< RigidBody* >( cm.mContect.object[1] );
if ( bodyA->getMotionType() == BodyMotion::eStatic ||
bodyB->getMotionType() == BodyMotion::eStatic )
{
sortedContact[idxStatic--] = &cm;
}
else
{
sortedContact[idxNormal++] = &cm;
}
}
for( int i = 0 ; i < numMainfold ; ++i )
{
ContactManifold& cm = *sortedContact[i];
Contact& c = cm.mContect;
Vec2f cp = 0.5 * ( c.pos[0] + c.pos[1] );
RigidBody* bodyA = static_cast< RigidBody* >( c.object[0] );
RigidBody* bodyB = static_cast< RigidBody* >( c.object[1] );
Vec2f vA = bodyA->getVelFromWorldPos( cp );
Vec2f vB = bodyB->getVelFromWorldPos( cp );
float vrel = c.normal.dot( vB - vA );
float relectParam = 1.0f;
cm.velParam = 0;
if ( vrel < -1 )
{
cm.velParam = -relectParam * vrel;
}
//cm.impulse = 0;
////warm start
Vec2f rA = cp - bodyA->mPosCenter;
Vec2f rB = cp - bodyB->mPosCenter;
Vec2f dp = cm.impulse * c.normal;
bodyA->mLinearVel -= dp * bodyA->mInvMass;
//bodyA->mAngularVel -= rA.cross( dp ) * bodyA->mInvI;
bodyB->mLinearVel += dp * bodyB->mInvMass;
//bodyB->mAngularVel += rB.cross( dp ) * bodyB->mInvI;
}
if ( numMainfold != 0 )
jumpDebug();
//std::sort( sortedContact.begin() , sortedContact.end() , DepthSort() );
for( int nIter = 0 ; nIter < vIterNum ; ++nIter )
{
for( int i = 0 ; i < numMainfold ; ++i )
{
ContactManifold& cm = *sortedContact[i];
Contact& c = cm.mContect;
RigidBody* bodyA = static_cast< RigidBody* >( c.object[0] );
RigidBody* bodyB = static_cast< RigidBody* >( c.object[1] );
Vec2f cp = 0.5 * ( c.pos[0] + c.pos[1] );
Vec2f vA = bodyA->getVelFromWorldPos( cp );
Vec2f vB = bodyB->getVelFromWorldPos( cp );
Vec2f rA = cp - bodyA->mPosCenter;
Vec2f rB = cp - bodyB->mPosCenter;
Vec2f vrel = vB - vA;
float vn = vrel.dot( c.normal );
float nrA = rA.cross( c.normal );
float nrB = rB.cross( c.normal );
float invMass = 0;
invMass += bodyA->mInvMass + bodyA->mInvI * nrA * nrA;
invMass += bodyB->mInvMass + bodyB->mInvI * nrB * nrB;
float impulse = -( vn - cm.velParam ) / invMass;
float newImpulse = Math::Max( cm.impulse + impulse , 0.0f );
impulse = newImpulse - cm.impulse;
bodyA->mLinearVel -= impulse * c.normal * bodyA->mInvMass;
//bodyA->mAngularVel -= impulse * nrA * bodyA->mInvI;
bodyB->mLinearVel += impulse * c.normal * bodyB->mInvMass;
//bodyB->mAngularVel += impulse * nrB * bodyB->mInvI;
cm.impulse = newImpulse;
float fa = impulse * nrA * bodyA->mInvI;
float fb = impulse * nrB * bodyB->mInvI;
if ( fa != 0 || fb !=0 )
{
int i = 1;
}
if ( numMainfold != 0 )
jumpDebug();
}
}
for( RigidBodyList::iterator iter = mRigidBodies.begin() ,itEnd = mRigidBodies.end();
iter != itEnd ; ++iter )
{
RigidBody* body = *iter;
body->intergedTramsform( dt );
//body->mAngularVel = 0;
}
for( int nIter = 0 ; nIter < pIterNum ; ++nIter )
{
float const kValueB = 0.8f;
float const kMaxDepth = 2.f;
float const kSlopValue = 0.0001f;
float maxDepth = 0.0;
for( int i = 0 ; i < numMainfold ; ++i )
{
ContactManifold& cm = *sortedContact[i];
Contact& c = cm.mContect;
RigidBody* bodyA = static_cast< RigidBody* >( c.object[0] );
RigidBody* bodyB = static_cast< RigidBody* >( c.object[1] );
if ( bodyB->getMotionType() != BodyMotion::eDynamic &&
bodyB->getMotionType() != BodyMotion::eDynamic )
continue;
Vec2f cpA = bodyA->mXForm.mul( c.posLocal[0] );
Vec2f cpB = bodyB->mXForm.mul( c.posLocal[1] );
//TODO: normal change need concerned
Vec2f normal = c.normal;
float depth = normal.dot( cpA - cpB ) + 0.001;
if ( depth <= 0 )
continue;
Vec2f cp = 0.5 * ( cpA + cpB );
Vec2f rA = cp - bodyA->mPosCenter;
Vec2f rB = cp - bodyB->mPosCenter;
float nrA = rA.cross( normal );
float nrB = rB.cross( normal );
float invMass = 0;
invMass += bodyA->mInvMass + bodyA->mInvI * nrA * nrA;
invMass += bodyB->mInvMass + bodyB->mInvI * nrB * nrB;
float offDepth = Math::Clamp ( ( depth - kSlopValue ) , 0 , kMaxDepth );
float impulse = ( invMass > 0 ) ? kValueB * offDepth / invMass : 0;
if ( impulse > 0 )
{
bodyA->mXForm.translate( -impulse * normal * bodyA->mInvMass );
bodyA->mRotationAngle += -impulse * nrA * bodyA->mInvI;
bodyA->synTransform();
bodyB->mXForm.translate( impulse * normal * bodyB->mInvMass );
bodyB->mRotationAngle += impulse * nrB * bodyB->mInvI;
bodyB->synTransform();
}
{
Vec2f cpA = bodyA->mXForm.mul( c.posLocal[0] );
Vec2f cpB = bodyB->mXForm.mul( c.posLocal[1] );
//TODO: normal change need concerned
float depth2 = normal.dot( cpA - cpB );
float dp = depth - depth2;
::Msg( "dp = %f " , dp );
int i = 1;
}
}
if ( maxDepth < 3 * kMaxDepth )
break;
}
if ( numMainfold != 0 )
jumpDebug();
}