Collisions work better

Still needs polishing

Code/Game/DanBiasGame/GameClientState/GameState.cpp

@@ -94,6 +94,9 @@ bool GameState::LoadModels(std::wstring mapFile)
 	C_Object* obj;
 	translate =  Oyster::Math3D::TranslationMatrix(Oyster::Math::Float3(0,0,0));
 	modelData.world = translate  ;//modelData.world * translate 
+	modelData.world.v[0].x = 2;
+	modelData.world.v[1].y = 2;
+	modelData.world.v[2].z = 2;
 	modelData.modelPath = L"world_earth.dan";
 	modelData.id = id++;
 
@@ -483,7 +486,7 @@ void GameState::Protocol( ObjPos* pos )
 
 				camera->setRight(right);
 				camera->setUp(up);
-				camera->setLook(objForward);
+				//camera->setLook(objForward);
 				
 				up *= 1;
 				objForward *= -2;

Code/Game/GameLogic/Game_PlayerData.cpp

@@ -7,7 +7,7 @@ Game::PlayerData::PlayerData()
 {	
 	//set some stats that are appropriate to a player
 	Oyster::Physics::API::SimpleBodyDescription sbDesc;
-	sbDesc.centerPosition = Oyster::Math::Float3(0,308,0);
+	sbDesc.centerPosition = Oyster::Math::Float3(0,608,0);
 	sbDesc.size = Oyster::Math::Float3(0.5f,2,1);
 	sbDesc.mass = 70;
 	sbDesc.restitutionCoeff = 0.5;

Code/Game/GameLogic/Level.cpp

@@ -54,7 +54,7 @@ void Level::InitiateLevel(float radius)
 	API::SphericalBodyDescription sbDesc;
 	sbDesc.centerPosition = Oyster::Math::Float4(0,0,0,1);
 	sbDesc.ignoreGravity = true;
-	sbDesc.radius = 300; 
+	sbDesc.radius = 600; 
 	sbDesc.mass = 100;
 	sbDesc.frictionCoeff_Static = 0;
 	sbDesc.frictionCoeff_Dynamic = 0;
@@ -76,7 +76,7 @@ void Level::InitiateLevel(float radius)
 	sbDesc_TestBox.ignoreGravity = false;
 
 	sbDesc_TestBox.mass = 20;
-	sbDesc_TestBox.size = Oyster::Math::Float4(1,1,1,0);
+	sbDesc_TestBox.size = Oyster::Math::Float4(0.5,0.5,0.5,0);
 
 	
 	ICustomBody* rigidBody_TestBox;
@@ -85,7 +85,7 @@ void Level::InitiateLevel(float radius)
 	int offset = 0;
 	for(int i =0; i< nrOfBoxex; i ++)
 	{
-		sbDesc_TestBox.centerPosition = Oyster::Math::Float4(0,305 + i*5,5,1);
+		sbDesc_TestBox.centerPosition = Oyster::Math::Float4(0,605 + i*5,5,1);
 		rigidBody_TestBox = API::Instance().CreateRigidBody(sbDesc_TestBox).Release();
 		rigidBody_TestBox->SetSubscription(Level::PhysicsOnMoveLevel);
 
@@ -95,7 +95,7 @@ void Level::InitiateLevel(float radius)
 	offset += nrOfBoxex;
 	for(int i =0; i< nrOfBoxex; i ++)
 	{
-		sbDesc_TestBox.centerPosition = Oyster::Math::Float4(-20,320, -200 +( i*7),0);
+		sbDesc_TestBox.centerPosition = Oyster::Math::Float4(-20,620, -200 +( i*7),0);
 		rigidBody_TestBox = API::Instance().CreateRigidBody(sbDesc_TestBox).Release();
 		rigidBody_TestBox->SetSubscription(Level::PhysicsOnMoveLevel);
 
@@ -105,7 +105,7 @@ void Level::InitiateLevel(float radius)
 	offset += nrOfBoxex;
 	for(int i =0; i< nrOfBoxex; i ++)
 	{
-		sbDesc_TestBox.centerPosition = Oyster::Math::Float4(200,320 + ( i*7),0,0);
+		sbDesc_TestBox.centerPosition = Oyster::Math::Float4(200,620 + ( i*7),0,0);
 		rigidBody_TestBox = API::Instance().CreateRigidBody(sbDesc_TestBox).Release();
 		rigidBody_TestBox->SetSubscription(Level::PhysicsOnMoveLevel);
 
@@ -115,7 +115,7 @@ void Level::InitiateLevel(float radius)
 	offset += nrOfBoxex;
 	for(int i =0; i< nrOfBoxex; i ++)
 	{
-		sbDesc_TestBox.centerPosition = Oyster::Math::Float4(5,305 + i*5,0,0);
+		sbDesc_TestBox.centerPosition = Oyster::Math::Float4(5,605 + i*5,0,0);
 		rigidBody_TestBox = API::Instance().CreateRigidBody(sbDesc_TestBox).Release();
 		rigidBody_TestBox->SetSubscription(Level::PhysicsOnMoveLevel);
 
@@ -129,7 +129,7 @@ void Level::InitiateLevel(float radius)
 
 	// add crystal
 	API::SimpleBodyDescription sbDesc_Crystal;
-	sbDesc_Crystal.centerPosition = Oyster::Math::Float4(10, 305, 0, 1);
+	sbDesc_Crystal.centerPosition = Oyster::Math::Float4(10, 605, 0, 1);
 	sbDesc_Crystal.ignoreGravity = false;
 	sbDesc_Crystal.mass = 80;
 	sbDesc_Crystal.size = Oyster::Math::Float3(1,2,1);
@@ -143,7 +143,7 @@ void Level::InitiateLevel(float radius)
 	// add house
 	API::SimpleBodyDescription sbDesc_House;
 	//sbDesc_House.centerPosition = Oyster::Math::Float4(212, 212, 0, 0);
-	sbDesc_House.centerPosition = Oyster::Math::Float4(-50, 290, 0, 1);
+	sbDesc_House.centerPosition = Oyster::Math::Float4(-50, 690, 0, 1);
 	sbDesc_House.ignoreGravity = false;
 	sbDesc_House.rotation = Oyster::Math::Float3(0 ,Utility::Value::Radian(90.0f), 0);
 	sbDesc_House.mass = 90;

Code/GamePhysics/Implementation/Octree.cpp

@@ -140,6 +140,7 @@ void Octree::Visit(ICustomBody* customBodyRef, VisitorAction hitAction )
 {
 	auto object = this->mapReferences.find(customBodyRef);
 
+	// If rigid body is not found
 	if(object == this->mapReferences.end())
 	{	
 		return;
@@ -147,8 +148,10 @@ void Octree::Visit(ICustomBody* customBodyRef, VisitorAction hitAction )
 
 	unsigned int tempRef = object->second;
 
+	// Go through all object and test for intersection
 	for(unsigned int i = 0; i<this->leafData.size(); i++)
 	{
+		// If objects intersect call collision response function
 		if(tempRef != i && !this->leafData[i].limbo) if(this->leafData[tempRef].container.Intersects(this->leafData[i].container))
 		{
 			hitAction(*this, tempRef, i);

Code/GamePhysics/Implementation/PhysicsAPI_Impl.cpp

@@ -166,11 +166,6 @@ namespace
 		auto proto = worldScene.GetCustomBody( protoTempRef );
 		auto deuter = worldScene.GetCustomBody( deuterTempRef );
 
-		if(proto->GetState().GetMass() == 70)
-		{
-			const char *breakpoint = "STOP";
-		}
-
 		Float4 worldPointOfContact;
 		if( proto->Intersects(*deuter, worldPointOfContact) )
 		{
@@ -178,25 +173,12 @@ namespace
 			ICustomBody::State protoState; proto->GetState( protoState );
 			ICustomBody::State deuterState; deuter->GetState( deuterState );
 
-			Float4 normal = (worldPointOfContact - Float4(deuterState.GetCenterPosition(), 1.0f )); // Init value is only borrowed
+			
-			if( normal.Dot(normal) > 0.0f )
+
+			Float4 normal = deuter->GetNormalAt(worldPointOfContact);
+			if(normal == Float4::null)
 			{
-				deuter->GetNormalAt( worldPointOfContact, normal );
+				normal = Float4(deuterState.GetCenterPosition(), 1) - Float4(protoState.GetCenterPosition(), 1);
-			}
-			else
-			{ // special case: deuter is completly contained within proto or they have overlapping centers.
-
-				normal = Float4( protoState.GetCenterPosition() - deuterState.GetCenterPosition(), 0.0f );
-				if( normal.Dot(normal) == 0.0f )
-				{ // they have overlapping centers. Rebound at least
-					// calculate and store time interpolation value, for later rebound.
-					proto->SetTimeOfContact( worldPointOfContact );
-					return;
-				}
-				
-				// borrowing the negated normal of proto.
-				proto->GetNormalAt( worldPointOfContact, normal );
-				normal = -normal;
 			}
 			normal.Normalize();
 
@@ -206,12 +188,16 @@ namespace
 			Float protoG_Magnitude = protoG.Dot( normal ),
 				  deuterG_Magnitude = deuterG.Dot( normal );
 
-			if(protoState.GetMass() == 20)
+			// If true the object is inside the world
+			if(worldPointOfContact.GetLength() < 600 && protoState.GetCenterPosition().GetLength() != 0)
 			{
-				const char *breakpoint = "STOP";
+				Float overlap = 600 - worldPointOfContact.GetLength();
+				Float3 newPos = overlap*worldPointOfContact.GetNormalized();
+				protoState.SetCenterPosition(protoState.GetCenterPosition() + newPos);
+				protoState.SetLinearMomentum(Float3(0, 0, 0));
 			}
 
-			// if they are not relatively moving towards eachother, there is no collision
+			// If they are not relatively moving towards eachother, there is no collision
 			Float deltaPos = normal.Dot( Float4(deuterState.GetCenterPosition(), 1) - Float4(protoState.GetCenterPosition(), 1) );
 			if( deltaPos < 0.0f )
 			{
@@ -232,64 +218,34 @@ namespace
 				return;
 			}
 
-			// bounce
+			// Proto
-			Float4 bounceD = normal * -Formula::CollisionResponse::Bounce( deuterState.GetRestitutionCoeff(),
+			normal = -proto->GetNormalAt(worldPointOfContact);
-																		   deuterState.GetMass(), deuterG_Magnitude,
+			if(normal == Float4::null)
-																		   protoState.GetMass(), protoG_Magnitude );
-
-			normal = (worldPointOfContact - Float4(protoState.GetCenterPosition(), 1.0f )).GetNormalized();
-			if( normal.Dot(normal) > 0.0f )
 			{
-				proto->GetNormalAt( worldPointOfContact, normal );
+				normal = Float4(protoState.GetCenterPosition(), 1) - Float4(deuterState.GetCenterPosition(), 1);	
-				protoG_Magnitude = protoG.Dot( normal );
-				deuterG_Magnitude = deuterG.Dot( normal );
-				normal.Normalize();
-			}
-			else
-			{ // special case: proto is completly contained within deuter.
-				// borrowing the negated normal of deuter.
-				deuter->GetNormalAt( worldPointOfContact, normal );
-				normal = -normal;
-				protoG_Magnitude = -protoG_Magnitude;
-				deuterG_Magnitude = -deuterG_Magnitude;
 			}
 			normal.Normalize();
 
-			if( normal != normal ) // debug: trap
+			// Calculate and apply friction to rigid body
-				const char *breakpoint = "This should never happen";
-
 			Float4 friction = Formula::CollisionResponse::Friction( protoG_Magnitude, normal,
 																	Float4(protoState.GetLinearMomentum(), 0),  protoState.GetFrictionCoeff_Static(),  protoState.GetFrictionCoeff_Kinetic(),  protoState.GetMass(), 
 																	Float4(deuterState.GetLinearMomentum(), 0), deuterState.GetFrictionCoeff_Static(), deuterState.GetFrictionCoeff_Kinetic(), deuterState.GetMass());
 
+			//protoState.ApplyFriction( -friction.xyz );
 
-
+			// If no other collision response is wanted then this will stop the bounce
-			protoState.ApplyFriction( -friction.xyz );
-
 			if( proto->CallSubscription_BeforeCollisionResponse(proto) == ICustomBody::SubscriptMessage_ignore_collision_response )
 			{
 				return;
 			}
 
-			// PLayerHAck
-			if( proto->CallSubscription_BeforeCollisionResponse(proto) == ICustomBody::SubscriptMessage_player_collision_response )
-			{
-				return;
-			}
 		
 			
-			
+			// Calaculate bounce
-			
+			Float4 bounce = normal * Formula::CollisionResponse::Bounce( protoState.GetRestitutionCoeff(),
-			// bounce
-			Float4 bounceP = normal * Formula::CollisionResponse::Bounce( protoState.GetRestitutionCoeff(),
 																		  protoState.GetMass(), protoG_Magnitude,
 																		  deuterState.GetMass(), deuterG_Magnitude );
-
+			// If bounce is not big enough to matter, set to 0
-			Float4 bounce = bounceP;
-
-			//LinearAlgebra3D::InterpolateAxisYToNormal_UsingNlerp(state.SetOrientation(, Float4(state.GetGravityNormal(), 0.0f), 1.0f);
-
-
 			if( abs(bounce.x) < 0.001 )
 			{
 				bounce.x = 0;
@@ -303,14 +259,23 @@ namespace
 				bounce.z = 0;
 			}
 
-			Float kineticEnergyPBefore = Oyster::Physics3D::Formula::LinearKineticEnergy( protoState.GetMass(), protoState.GetLinearMomentum()/protoState.GetMass() );
+			if( bounce != bounce)
+			{
+				const char* breakpoint = "STOP";
+			}
 
+			// Calculate kinetic energy before impulse is applied
+			Float kineticEnergyBefore = Oyster::Physics3D::Formula::LinearKineticEnergy( protoState.GetMass(), protoState.GetLinearMomentum()/protoState.GetMass() );
+
+			// Apply the bounce as impulse
 			protoState.ApplyImpulse( bounce.xyz, worldPointOfContact.xyz, normal.xyz );
 			proto->SetState( protoState );
 
-			Float kineticEnergyPAFter = Oyster::Physics3D::Formula::LinearKineticEnergy( protoState.GetMass(), (protoState.GetLinearMomentum() + protoState.GetLinearImpulse())/protoState.GetMass() );
+			// Calculate kinetic energy after impulse is applied
+			Float kineticEnergyAfter = Oyster::Physics3D::Formula::LinearKineticEnergy( protoState.GetMass(), (protoState.GetLinearMomentum() + protoState.GetLinearImpulse())/protoState.GetMass() );
 
-			proto->CallSubscription_AfterCollisionResponse( deuter,  kineticEnergyPBefore - kineticEnergyPAFter );
+			// Call a collision function with kinetic energy loss
+			proto->CallSubscription_AfterCollisionResponse( deuter,  kineticEnergyBefore - kineticEnergyAfter );
 		}
 	}
 }
@@ -373,91 +338,50 @@ float API_Impl::GetFrameTimeLength() const
 }
 
 void API_Impl::Update()
-{ /** @todo TODO: Update is a temporary solution .*/
+{ 
-	::std::vector<ICustomBody*> updateList;
-	this->worldScene.Sample( Universe(), updateList );
 	ICustomBody::State state;
+	::std::vector<ICustomBody*> updateList;
+
+	// Fetch objects in universe
+	this->worldScene.Sample( Universe(), updateList );
+
+	// Change momentum for all rigid bodies
 	for( int i = 0; i < updateList.size(); i++ )
 	{
-		auto proto = updateList[i];
+		ICustomBody* proto = updateList[i];
-		// Step 1: Apply Gravity
+		// Step 1: Apply gravity to rigid body
 		Float4 gravityImpulse = Float4::null;
 		proto->GetState( state );
-		for( int j = 0; j < this->gravity.size(); ++j )
+
+		Float4 deltaPosGrav = Float4( this->gravity[0].well.position, 1.0f ) - Float4( state.GetCenterPosition(), 1.0f );
+		Float rSquared = deltaPosGrav.Dot( deltaPosGrav );
+		if( rSquared != 0.0 )
 		{
-			switch( this->gravity[j].gravityType )
+			Float force = 9.82*10;
-			{
+			gravityImpulse += (this->updateFrameLength*force)*deltaPosGrav.GetNormalized();
-			case Gravity::GravityType_Well:
-				{
-					Float4 d = Float4( this->gravity[j].well.position, 1.0f ) - Float4( state.GetCenterPosition(), 1.0f );
-					Float rSquared = d.Dot( d );
-					if( rSquared != 0.0 )
-					{
-						if(state.GetMass() == 70)
-						{
-							const char *breakpoint = "STOP";
-						}
-						Float force = 9.82*10;
-						gravityImpulse += ((this->updateFrameLength * force)) * d.GetNormalized();
-					}
-					break;
-				}
-			case Gravity::GravityType_Directed:
-				gravityImpulse += Float4( this->gravity[j].directed.impulse, 0.0f );
-				break;
-//			case Gravity::GravityType_DirectedField:
-//				//this->gravity[i].directedField.
-//				//! TODO: @todo rethink
-//				break;
-			default: break;
-			}
 		}
 
-		if( gravityImpulse != gravityImpulse ) // debug: trap
-				const char *breakpoint = "This should never happen";
 		Float posLength = state.GetCenterPosition().GetLength();
-		if( gravityImpulse != Float4::null && posLength - 300 > state.GetReach().y )
+		if( gravityImpulse != Float4::null && posLength - 601 > state.GetReach().GetLength() )
 		{
 			state.ApplyLinearImpulse( gravityImpulse.xyz );
 			state.SetGravityNormal( gravityImpulse.GetNormalized().xyz );
 			proto->SetState( state );
 		}
-		if(state.GetMass() == 70)
+
-		{
+		// Step 2: Step through octree and apply collision responses to rigid body
-			const char *breakpoint = "STOP";
-		}
-		// Step 2: Apply Collision Response
 		this->worldScene.Visit( proto, OnPossibleCollision );
 	}
 
-	
+	// Go through all rigid bodies and move them according to their momentums
 	for( int i = 0; i < updateList.size(); i++ )
 	{
 		auto proto = updateList[i];
-		proto->GetState( state );
-		Float3 lM = state.GetLinearMomentum();
-
-		//LinearAlgebra3D::InterpolateAxisYToNormal_UsingNlerp(state.SetOrientation(, Float4(state.GetGravityNormal(), 0.0f), 1.0f);
-
-
-		if( abs(lM.x) < this->epsilon )
-		{
-			state.linearMomentum.x = 0;
-		}
-		if( abs(lM.y) < this->epsilon )
-		{
-			state.linearMomentum.y = 0;
-		}
-		if( abs(lM.z) < this->epsilon )
-		{
-			state.linearMomentum.z = 0;
-		}
-
-		proto->SetState( state );
 
 		switch( proto->Update(this->updateFrameLength) )
 		{
 		case UpdateState_altered:
+			// Moves the container in the octree to the new rigid body position
 			this->worldScene.SetAsAltered( this->worldScene.GetTemporaryReferenceOf(proto) );
 			proto->CallSubscription_Move();
 		case UpdateState_resting: 

Code/GamePhysics/Implementation/SimpleRigidBody.cpp

@@ -111,6 +111,11 @@ SimpleRigidBody::SimpleRigidBody( const API::SimpleBodyDescription &desc )
 	this->scene = nullptr;
 	this->customTag = nullptr;
 	this->ignoreGravity = desc.ignoreGravity;
+
+	this->collisionRebound.previousSpatial.center = this->rigid.centerPos;
+	this->collisionRebound.previousSpatial.axis = this->rigid.axis;
+	this->collisionRebound.previousSpatial.reach = this->rigid.boundingReach;
+	this->collisionRebound.timeOfContact = 1.0f;
 }
 
 SimpleRigidBody::~SimpleRigidBody() {}

Code/GamePhysics/Implementation/SphericalRigidBody.cpp

@@ -98,7 +98,8 @@ SphericalRigidBody::State SphericalRigidBody::GetState() const
 				  this->rigid.frictionCoeff_Static, this->rigid.frictionCoeff_Kinetic,
 				  this->rigid.GetMomentOfInertia(), this->rigid.boundingReach,
 				  this->rigid.centerPos, this->rigid.axis,
-				  this->rigid.momentum_Linear, this->rigid.momentum_Angular );
+				  this->rigid.momentum_Linear, this->rigid.momentum_Angular,
+				  this->gravityNormal );
 }
 
 SphericalRigidBody::State & SphericalRigidBody::GetState( SphericalRigidBody::State &targetMem ) const
@@ -107,7 +108,8 @@ SphericalRigidBody::State & SphericalRigidBody::GetState( SphericalRigidBody::St
 							  this->rigid.frictionCoeff_Static, this->rigid.frictionCoeff_Kinetic,
 							  this->rigid.GetMomentOfInertia(), this->rigid.boundingReach,
 							  this->rigid.centerPos, this->rigid.axis,
-							  this->rigid.momentum_Linear, this->rigid.momentum_Angular );
+							  this->rigid.momentum_Linear, this->rigid.momentum_Angular,
+							  this->gravityNormal );
 }
 
 void SphericalRigidBody::SetState( const SphericalRigidBody::State &state )
@@ -299,6 +301,11 @@ void SphericalRigidBody::Predict( ::Oyster::Math::Float4 &outDeltaPos, ::Oyster:
 	this->rigid.Predict_LeapFrog( outDeltaPos.xyz, outDeltaAxis.xyz, actingLinearImpulse.xyz, actingAngularImpulse.xyz, deltaTime );
 }
 
+void SphericalRigidBody::SetScene( void *scene )
+{
+	this->scene = (Octree*)scene;
+}
+
 void SphericalRigidBody::SetSubscription( ICustomBody::EventAction_BeforeCollisionResponse functionPointer )
 {
 	if( functionPointer )
@@ -335,11 +342,6 @@ void SphericalRigidBody::SetSubscription( ICustomBody::EventAction_Move function
 	}
 }
 
-void SphericalRigidBody::SetScene( void *scene )
-{
-	this->scene = (Octree*)scene;
-}
-
 void SphericalRigidBody::SetGravity( bool ignore )
 {
 	this->ignoreGravity = ignore;
@@ -349,6 +351,7 @@ void SphericalRigidBody::SetGravity( bool ignore )
 void SphericalRigidBody::SetGravityNormal( const Float3 &normalizedVector )
 {
 	this->gravityNormal = normalizedVector;
+	this->rigid.gravityNormal = Float4( this->gravityNormal, 0 );
 }
 
 void SphericalRigidBody::SetCustomTag( void *ref )

Code/OysterPhysics3D/RigidBody.cpp

@@ -56,7 +56,7 @@ void RigidBody::Update_LeapFrog( Float updateFrameLength )
 
 	// ds = dt * Formula::LinearVelocity( m, avg_G ) = dt * avg_G / m = (dt / m) * avg_G
 	Float3 delta = AverageWithDelta( this->momentum_Linear, this->impulse_Linear );
-	Float3 newPos = ( updateFrameLength)*this->momentum_Linear;
+	Float3 newPos = updateFrameLength*this->momentum_Linear;
 	this->centerPos += newPos;
 	
 	if(this->mass == 70)
@@ -66,7 +66,7 @@ void RigidBody::Update_LeapFrog( Float updateFrameLength )
 
 	// updating the angular
 	// dO = dt * Formula::AngularVelocity( (RI)^-1, avg_H ) = dt * (RI)^-1 * avg_H	
-	this->axis += updateFrameLength * this->momentOfInertiaTensor.CalculateAngularVelocity( this->rotation, AverageWithDelta(this->momentum_Angular, this->impulse_Angular) );
+	this->axis += updateFrameLength*this->momentOfInertiaTensor.CalculateAngularVelocity( this->rotation, this->momentum_Angular );
 	this->rotation = Rotation( this->axis );
 
 	// update momentums and clear impulse_Linear and impulse_Angular