From ab9f81ac60f8c8743b8589ebe617a53e081a4ec9 Mon Sep 17 00:00:00 2001 From: Robin Engman Date: Fri, 7 Feb 2014 15:38:53 +0100 Subject: [PATCH] Collisions work better Still needs polishing --- .../DanBiasGame/GameClientState/GameState.cpp | 5 +- Code/Game/GameLogic/Game_PlayerData.cpp | 2 +- Code/Game/GameLogic/Level.cpp | 16 +- Code/GamePhysics/Implementation/Octree.cpp | 3 + .../Implementation/PhysicsAPI_Impl.cpp | 186 ++++++------------ .../Implementation/SimpleRigidBody.cpp | 5 + .../Implementation/SphericalRigidBody.cpp | 17 +- Code/OysterPhysics3D/RigidBody.cpp | 4 +- 8 files changed, 88 insertions(+), 150 deletions(-) diff --git a/Code/Game/DanBiasGame/GameClientState/GameState.cpp b/Code/Game/DanBiasGame/GameClientState/GameState.cpp index 8b47400a..06def943 100644 --- a/Code/Game/DanBiasGame/GameClientState/GameState.cpp +++ b/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; diff --git a/Code/Game/GameLogic/Game_PlayerData.cpp b/Code/Game/GameLogic/Game_PlayerData.cpp index ee535868..e4b54f9a 100644 --- a/Code/Game/GameLogic/Game_PlayerData.cpp +++ b/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; diff --git a/Code/Game/GameLogic/Level.cpp b/Code/Game/GameLogic/Level.cpp index 14a5d140..9dc4a6f8 100644 --- a/Code/Game/GameLogic/Level.cpp +++ b/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; diff --git a/Code/GamePhysics/Implementation/Octree.cpp b/Code/GamePhysics/Implementation/Octree.cpp index 47bf3e39..e615df63 100644 --- a/Code/GamePhysics/Implementation/Octree.cpp +++ b/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; ileafData.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); diff --git a/Code/GamePhysics/Implementation/PhysicsAPI_Impl.cpp b/Code/GamePhysics/Implementation/PhysicsAPI_Impl.cpp index 0203f87a..a59c03b1 100644 --- a/Code/GamePhysics/Implementation/PhysicsAPI_Impl.cpp +++ b/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 ) - { - deuter->GetNormalAt( worldPointOfContact, normal ); - } - 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; + Float4 normal = deuter->GetNormalAt(worldPointOfContact); + if(normal == Float4::null) + { + normal = Float4(deuterState.GetCenterPosition(), 1) - Float4(protoState.GetCenterPosition(), 1); } 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 - Float4 bounceD = normal * -Formula::CollisionResponse::Bounce( deuterState.GetRestitutionCoeff(), - deuterState.GetMass(), deuterG_Magnitude, - protoState.GetMass(), protoG_Magnitude ); - - normal = (worldPointOfContact - Float4(protoState.GetCenterPosition(), 1.0f )).GetNormalized(); - if( normal.Dot(normal) > 0.0f ) + // Proto + normal = -proto->GetNormalAt(worldPointOfContact); + if(normal == Float4::null) { - proto->GetNormalAt( worldPointOfContact, normal ); - 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 = Float4(protoState.GetCenterPosition(), 1) - Float4(deuterState.GetCenterPosition(), 1); } normal.Normalize(); - - if( normal != normal ) // debug: trap - const char *breakpoint = "This should never happen"; + // Calculate and apply friction to rigid body 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 ); - - protoState.ApplyFriction( -friction.xyz ); - + // If no other collision response is wanted then this will stop the bounce if( proto->CallSubscription_BeforeCollisionResponse(proto) == ICustomBody::SubscriptMessage_ignore_collision_response ) { return; } - // PLayerHAck - if( proto->CallSubscription_BeforeCollisionResponse(proto) == ICustomBody::SubscriptMessage_player_collision_response ) - { - return; - } - - + - - // bounce - Float4 bounceP = normal * Formula::CollisionResponse::Bounce( protoState.GetRestitutionCoeff(), + // Calaculate bounce + Float4 bounce = normal * Formula::CollisionResponse::Bounce( protoState.GetRestitutionCoeff(), protoState.GetMass(), protoG_Magnitude, deuterState.GetMass(), deuterG_Magnitude ); - - Float4 bounce = bounceP; - - //LinearAlgebra3D::InterpolateAxisYToNormal_UsingNlerp(state.SetOrientation(, Float4(state.GetGravityNormal(), 0.0f), 1.0f); - - + // If bounce is not big enough to matter, set to 0 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 updateList; - this->worldScene.Sample( Universe(), updateList ); +{ ICustomBody::State state; + ::std::vector 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]; - // Step 1: Apply Gravity + ICustomBody* proto = updateList[i]; + // 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 ) - { - 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; - } + Float force = 9.82*10; + gravityImpulse += (this->updateFrameLength*force)*deltaPosGrav.GetNormalized(); } - 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) - { - const char *breakpoint = "STOP"; - } - // Step 2: Apply Collision Response + + // Step 2: Step through octree and apply collision responses to rigid body 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: diff --git a/Code/GamePhysics/Implementation/SimpleRigidBody.cpp b/Code/GamePhysics/Implementation/SimpleRigidBody.cpp index f1c196f7..68759398 100644 --- a/Code/GamePhysics/Implementation/SimpleRigidBody.cpp +++ b/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() {} diff --git a/Code/GamePhysics/Implementation/SphericalRigidBody.cpp b/Code/GamePhysics/Implementation/SphericalRigidBody.cpp index 0ecb8b6e..b27d3a78 100644 --- a/Code/GamePhysics/Implementation/SphericalRigidBody.cpp +++ b/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 ) diff --git a/Code/OysterPhysics3D/RigidBody.cpp b/Code/OysterPhysics3D/RigidBody.cpp index f7a5646e..ab0b1a18 100644 --- a/Code/OysterPhysics3D/RigidBody.cpp +++ b/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