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(); }