Exemple #1
0
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);
					}
				}
			}
		}
Exemple #4
0
	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();

	}