diff --git a/Code/Dokumentation/Other/Timestep_Impulse_Fix.odt b/Code/Dokumentation/Other/Timestep_Impulse_Fix.odt new file mode 100644 index 00000000..25ac526e Binary files /dev/null and b/Code/Dokumentation/Other/Timestep_Impulse_Fix.odt differ diff --git a/Code/Dokumentation/Other/Timestep_Impulse_Fix.pdf b/Code/Dokumentation/Other/Timestep_Impulse_Fix.pdf new file mode 100644 index 00000000..8ee3faea Binary files /dev/null and b/Code/Dokumentation/Other/Timestep_Impulse_Fix.pdf differ diff --git a/Code/Dokumentation/Other/angular momentum to angular velocity.odt b/Code/Dokumentation/Other/angular momentum to angular velocity.odt new file mode 100644 index 00000000..9cb22720 Binary files /dev/null and b/Code/Dokumentation/Other/angular momentum to angular velocity.odt differ diff --git a/Code/Dokumentation/Other/angular_momentum_to_angular_velocity.pdf b/Code/Dokumentation/Other/angular_momentum_to_angular_velocity.pdf new file mode 100644 index 00000000..820bf412 Binary files /dev/null and b/Code/Dokumentation/Other/angular_momentum_to_angular_velocity.pdf differ diff --git a/Code/Dokumentation/Physics_Sprint3.uxf b/Code/Dokumentation/Physics_Sprint3.uxf new file mode 100644 index 00000000..6b74de3e --- /dev/null +++ b/Code/Dokumentation/Physics_Sprint3.uxf @@ -0,0 +1,1059 @@ + + + 7 + + + com.umlet.element.Class + + 357 + 518 + 252 + 406 + + Formula : <<namespace>> +<<extern>> +-- +LinearMomentum( .. ) : Vector +LinearVelocity( .. ) : Vector +AngularMomentum( .. ) : Vector +AngularVelocity( .. ) : Vector +TangientialLinearMomentum( .. ) : Vector +LinearKineticEnergy( .. ) : Float +AngularKineticEnergy( .. ) : Float +TangientialImpulseForce( .. ) : Vector +AngularImpulseAcceleration( .. ) : Vector +TangientialImpulseAcceleration( .. ) : Vector +TangientialLinearVelocity( .. ) : Vector +LinearImpulseAcceleration( .. ) : Vector +ImpulseForce( .. ) : Vector +ImpulseTorque( .. ) : Vector +Forcefield( .. ) : Float + + + + com.umlet.element.Class + + 364 + 728 + 238 + 189 + + MomentOfInertia : <<namespace>> +<<extern>> +-- +CalculateSphere( .. ) : Float +Sphere( .. ) : Matrix +CalculateHollowSphere( .. ) : Float +HollowSphere( .. ) : Matrix +CalculateCuboidX( .. ) : Float +CalculateCuboidY( .. ) : Float +CalculateCuboidZ( .. ) : Float +Cuboid( .. ) : Matrix +CalculateRodCenter( .. ) : Float +RodCenter( .. ) : Matrix +CalculateCylinderXY( .. ) : Float +CalculateCylinderZ( .. ) : Float +Cylinder( .. ) : Matrix + + + + com.umlet.element.Class + + 616 + 518 + 210 + 35 + + RigidBody : struct +-- +Update_LeapFrog( deltatime : Float ) : void + + + + com.umlet.element.Class + + 616 + 581 + 210 + 203 + + MomentOfInertia : struct +-- +CalculateAngularVelocity( .. ) : Vector +CalculateAngularMomentum( .. ) : Vector +-- +<<static>> CalculateSphere( .. ) : Float +<<static>> Sphere( .. ) : MomentOfInertia +<<static>> CalculateHollowSphere( .. ) : Float +<<static>> HollowSphere( .. ) : MomentOfInertia +<<static>> CalculateCuboidX( .. ) : Float +<<static>> CalculateCuboidY( .. ) : Float +<<static>> CalculateCuboidZ( .. ) : Float +<<static>> Cuboid( .. ) : MomentOfInertia +<<static>> CalculateRodCenter( .. ) : Float +<<static>> RodCenter( .. ) : MomentOfInertia +<<static>> CalculateCylinderXY( .. ) : Float +<<static>> CalculateCylinderZ( .. ) : Float +<<static>> Cylinder( .. ) : MomentOfInertia + + + + com.umlet.element.Class + + 728 + 847 + 98 + 21 + + FluidDrag : struct + + + + com.umlet.element.Class + + 728 + 875 + 98 + 21 + + Particle : struct + + + + com.umlet.element.Class + + 728 + 903 + 98 + 21 + + Spring : struct + + + + com.umlet.element.Relation + + 644 + 532 + 132 + 62 + + lt=<- +<<uses>> + 119;49;119;35;21;35;21;21 + + + com.umlet.element.Class + + 350 + 483 + 483 + 448 + + Physics3D : <<namespace>> +<<extern>> +bg=green +-- + + + + + com.umlet.element.Class + + 560 + 420 + 161 + 49 + + Constant : <<namespace>> +<<extern>> +-- +gravity_constant : const Float + + + + + com.umlet.element.Class + + 224 + 385 + 329 + 84 + + Default : <<namespace>> +<<intern>> +-- +EventAction_Destruction : <<PhysicsAPI::EventAction_Destruction>> +EventAction_Collision : <<ICustomBody::EventAction_Collision>> +EventAction_CollisionResponse : <<ICustomBody::EventAction_Collision>> +EventAction_Move : <<ICustomBody::EventAction_Collision>> + + + + + com.umlet.element.Class + + 245 + 63 + 224 + 196 + + API : <<interface>> +-- +{innerclass + EventAction_Destruction : <<FunctionPointer>> +innerclass} + +Init( .. ) : void +SetFrameTimeLength( .. ) : void +SetGravityConstant( .. ) : void +SetSubscription( .. ) : void +Update() : void +IsInLimbo( .. ) : bool +MoveToLimbo( .. ) : void +ReleaseFromLimbo( .. ) : void +AddObject( .. ) : void +ExtractObject( .. ) : ICustomBody* +DestroyObject( .. ) : void +AddGravity( .. ) : void +RemoveGravity( .. ) : void +ApplyEffect( .. ) : void +CreateRigidBody( .. ) : ICustomBody* + + + + com.umlet.element.Class + + 273 + 280 + 133 + 28 + + API_Impl : <<class>> +-- +<<uses>> OctTree : class + + + + com.umlet.element.Class + + 476 + 63 + 280 + 287 + + ICustomBody : <<interface>> + +{innerclass + SubscriptMessage : Enum +innerclass} + +{innerclass + EventAction_Collision : <<Subscription : SubscriptMessage>> +innerclass} + +{innerclass + EventAction_CollisionResponse : <<Subscription : void>> +innerclass} + +{innerclass + EventAction_Move : <<Subscription : void>> +innerclass} + +Clone() : ICustomBody* +CallSubscription_Collision( .. ) : SubscriptMessage +CallSubscription_CollisionResponse( .. ) : void +CallSubscription_Move() : void +GetState( .. ) : State +SetState( .. ) : void +IsAffectedByGravity() : bool +Intersects( .. ) : bool +GetBoundingSphere : Sphere +GetNormalAt( .. ) : Vector +GetGravityNormal( .. ) : Vector +GetCustomTag() : void* +Update( .. ) : UpdateState +Predict( .. ) : void +SetScene( .. ) : void +SetSubscription( .. ) : void +SetGravity( .. ) : void +SetGravityNormal( .. ) : void +SetCustomTag( .. ) : void + + + + com.umlet.element.Class + + 273 + 315 + 175 + 28 + + SimpleRigidBody : <<class>> +-- +<<uses>> Physics3D::RigidBody : struct + + + + com.umlet.element.Class + + 273 + 350 + 175 + 28 + + SphericalRigidBody : <<class>> +-- +<<uses>> Physics3D::RigidBody : struct + + + + com.umlet.element.Class + + 21 + 273 + 196 + 196 + + Formula : <<namespace>> +<<intern>> +-- + + + + + com.umlet.element.Class + + 28 + 308 + 175 + 84 + + MomentOfInertia : <<namespace>> +<<intern>> +-- +CreateSphereMatrix( .. ) : Matrix +CreateHollowSphereMatrix( .. ) : Matrix +CreateCuboidMatrix( .. ) : Matrix +CreateCylinderMatrix( .. ) : Matrix +CreateRodMatrix( .. ) : Matrix + + + + com.umlet.element.Class + + 28 + 406 + 175 + 56 + + CollisionResponse : <<namespace>> +<<intern>> +-- +Bounce( .. ) : Float +Friction( .. ) : Vector + + + + com.umlet.element.Relation + + 308 + 238 + 34 + 55 + + lt=<<. + 21;21;21;42 + + + com.umlet.element.Relation + + 427 + 308 + 97 + 76 + + lt=<<. + 84;42;84;63;35;63;35;21;21;21 + + + com.umlet.element.Class + + 560 + 371 + 203 + 42 + + OctTree : <<class>> +-- +<<uses>> Collision3D::Sphere : struct +<<uses>> Collision3D::BoxAxisAligned : struct + + + + com.umlet.element.Relation + + 518 + 329 + 100 + 55 + + lt=<- + <<uses>> + 56;21;56;42 + + + com.umlet.element.Relation + + 427 + 329 + 97 + 55 + + lt=. + 84;21;84;42;21;42 + + + com.umlet.element.Class + + 21 + 63 + 182 + 196 + + Struct : <<namespace>> +<<extern>> +-- + + + + + com.umlet.element.Class + + 28 + 203 + 168 + 14 + + SimpleBodyDescription : struct + + + + com.umlet.element.Class + + 28 + 231 + 168 + 14 + + SimpleSphericalDescription : struct + + + + com.umlet.element.Class + + 28 + 175 + 168 + 14 + + CustomBodyState : struct + + + + com.umlet.element.Class + + 28 + 91 + 168 + 63 + + Gravity : struct +{innerclass + GravityWell +innerclass} +{innerclass + GravityDirected +innerclass} +{innerclass + GravityDirectedField +innerclass} + + + + com.umlet.element.Relation + + 182 + 105 + 104 + 195 + + lt=<- + <<uses>> + 21;21;56;21;56;168;91;168;91;182 + + + com.umlet.element.Relation + + 182 + 161 + 104 + 139 + + lt=<- + 21;21;56;21;56;112;91;112;91;126 + + + com.umlet.element.Relation + + 385 + 252 + 104 + 49 + + lt=<- +<<uses>> + 91;35;21;35 + + + com.umlet.element.Relation + + 182 + 189 + 104 + 181 + + lt=<- + 21;21;42;21;42;168;91;168 + + + com.umlet.element.Relation + + 175 + 217 + 111 + 153 + + lt=<- + <<uses>> + 28;21;49;21;49;140;98;140 + + + com.umlet.element.Relation + + 231 + 301 + 55 + 69 + + lt=- + 42;21;42;21;21;21;21;56 + + + com.umlet.element.Class + + 14 + 35 + 756 + 441 + + Physics : <<namespace>> +<<extern>> +bg=green +-- + + + + + com.umlet.element.Class + + 0 + 0 + 847 + 945 + + Oyster : <<namespace>> +<<extern>> +bg=orange +-- + + + + + + + com.umlet.element.Class + + 175 + 546 + 126 + 21 + + Box : class + + + + com.umlet.element.Class + + 175 + 518 + 126 + 21 + + ICollideAble : <<interface>> + + + + com.umlet.element.Class + + 175 + 574 + 126 + 21 + + AxisAlignedBox : class + + + + com.umlet.element.Class + + 175 + 602 + 126 + 21 + + Point : class + + + + com.umlet.element.Class + + 175 + 630 + 126 + 21 + + Sphere : class + + + + com.umlet.element.Class + + 175 + 770 + 126 + 21 + + Frustrum : class + + + + com.umlet.element.Class + + 175 + 658 + 126 + 21 + + Ray : class + + + + com.umlet.element.Class + + 175 + 700 + 126 + 21 + + Line : class + + + + com.umlet.element.Class + + 175 + 728 + 126 + 21 + + Plane : class + + + + com.umlet.element.Class + + 175 + 798 + 126 + 21 + + Universe : class + + + + com.umlet.element.Class + + 21 + 518 + 126 + 70 + + Utility : <<namespace>> +<<extern>> +-- +Compare( .. ) : void +Intersect( .. ) : bool +Contain( .. ) : bool + + + + com.umlet.element.Relation + + 280 + 504 + 55 + 321 + + lt=<<. + 21;21;42;21;42;308;21;308 + + + com.umlet.element.Relation + + 280 + 721 + 55 + 34 + + lt=. + 42;21;21;21 + + + com.umlet.element.Relation + + 280 + 693 + 55 + 34 + + lt=. + 42;21;21;21 + + + com.umlet.element.Relation + + 280 + 651 + 55 + 34 + + lt=. + 42;21;21;21 + + + com.umlet.element.Relation + + 280 + 763 + 55 + 34 + + lt=. + 42;21;21;21 + + + com.umlet.element.Relation + + 280 + 623 + 55 + 34 + + lt=. + 42;21;21;21 + + + com.umlet.element.Relation + + 280 + 595 + 55 + 34 + + lt=. + 42;21;21;21 + + + com.umlet.element.Relation + + 280 + 567 + 55 + 34 + + lt=. + 42;21;21;21 + + + com.umlet.element.Relation + + 280 + 539 + 55 + 34 + + lt=. + 42;21;21;21 + + + com.umlet.element.Relation + + 196 + 658 + 83 + 55 + + lt=<- +<<uses>> + 21;21;21;35;70;35;70;42 + + + com.umlet.element.Relation + + 35 + 567 + 153 + 84 + + lt=<- + <<uses>> + 49;21;49;70;140;70 + + + com.umlet.element.Relation + + 133 + 504 + 55 + 286 + + lt=- + 42;21;21;21;21;273;42;273 + + + com.umlet.element.Relation + + 196 + 728 + 83 + 55 + + lt=<- +<<uses>> + 21;21;21;35;70;35;70;42 + + + com.umlet.element.Relation + + 133 + 714 + 55 + 34 + + lt=- + 42;21;21;21 + + + com.umlet.element.Relation + + 133 + 644 + 55 + 34 + + lt=- + 42;21;21;21 + + + com.umlet.element.Relation + + 133 + 588 + 55 + 34 + + lt=- + 42;21;21;21 + + + com.umlet.element.Relation + + 133 + 560 + 55 + 34 + + lt=- + 42;21;21;21 + + + com.umlet.element.Relation + + 133 + 532 + 55 + 34 + + lt=- + 42;21;21;21 + + + com.umlet.element.Class + + 14 + 483 + 329 + 343 + + Collision3D : <<namespace>> +<<extern>> +bg=green +-- + + + + + + com.umlet.element.Class + + 938 + 77 + 98 + 28 + + OysterPhysics3D +<<lib>> +bg=green + + + + com.umlet.element.Class + + 896 + 175 + 42 + 21 + + Misc +bg=green + + + + com.umlet.element.Class + + 952 + 175 + 70 + 21 + + OysterMath +bg=green + + + + com.umlet.element.Relation + + 896 + 98 + 34 + 90 + + lt=()) + 21;77;21;21 + + + com.umlet.element.Relation + + 966 + 98 + 28 + 84 + + lt=()) + 21;77;21;21 + + + com.umlet.element.Class + + 896 + 28 + 161 + 28 + + GamePhysics +<<dll>> +bg=green + + + + com.umlet.element.Relation + + 896 + 35 + 48 + 97 + + lt=- + 35;84;21;84;21;21 + + + com.umlet.element.Class + + 889 + 0 + 175 + 133 + + Project Dependencies +-- +bg=blue + + + + com.umlet.element.Relation + + 910 + 84 + 90 + 48 + + lt=- + 21;35;77;35;77;21 + + + com.umlet.element.Relation + + 1036 + 21 + 105 + 28 + + lt=<() +r1=provide + 98;21;21;21 + + + com.umlet.element.Relation + + 1015 + 70 + 126 + 28 + + lt=<() +r1=provide + 119;21;21;21 + + + diff --git a/Code/GamePhysics/Implementation/Octree.cpp b/Code/GamePhysics/Implementation/Octree.cpp index 3faff29e..b4395516 100644 --- a/Code/GamePhysics/Implementation/Octree.cpp +++ b/Code/GamePhysics/Implementation/Octree.cpp @@ -128,13 +128,13 @@ void Octree::Visit(ICustomBody* customBodyRef, VisitorAction hitAction ) } } -void Octree::Visit(const Oyster::Collision3D::ICollideable& collideable, VisitorActionCollideable hitAction) +void Octree::Visit(const Oyster::Collision3D::ICollideable& collideable, void* args, VisitorActionCollideable hitAction) { for(unsigned int i = 0; ileafData.size(); i++) { if(collideable.Intersects(this->leafData[i].container)) { - hitAction( this->GetCustomBody(i) ); + hitAction( this->GetCustomBody(i), args ); } } } diff --git a/Code/GamePhysics/Implementation/Octree.h b/Code/GamePhysics/Implementation/Octree.h index 7b350795..50b9569a 100644 --- a/Code/GamePhysics/Implementation/Octree.h +++ b/Code/GamePhysics/Implementation/Octree.h @@ -18,7 +18,7 @@ namespace Oyster static const unsigned int invalid_ref; typedef void(*VisitorAction)(Octree&, unsigned int, unsigned int); - typedef void(*VisitorActionCollideable)(ICustomBody*); + typedef void(*VisitorActionCollideable)(ICustomBody*, void*); struct Data { @@ -53,7 +53,7 @@ namespace Oyster std::vector& Sample(ICustomBody* customBodyRef, std::vector& updateList); std::vector& Sample(const Oyster::Collision3D::ICollideable& collideable, std::vector& updateList); void Visit(ICustomBody* customBodyRef, VisitorAction hitAction ); - void Visit(const Oyster::Collision3D::ICollideable& collideable, VisitorActionCollideable hitAction ); + void Visit(const Oyster::Collision3D::ICollideable& collideable, void* args, VisitorActionCollideable hitAction ); ICustomBody* GetCustomBody(const unsigned int tempRef); diff --git a/Code/GamePhysics/Implementation/PhysicsAPI_Impl.cpp b/Code/GamePhysics/Implementation/PhysicsAPI_Impl.cpp index ead748e4..14c4d9e4 100644 --- a/Code/GamePhysics/Implementation/PhysicsAPI_Impl.cpp +++ b/Code/GamePhysics/Implementation/PhysicsAPI_Impl.cpp @@ -31,16 +31,16 @@ namespace ICustomBody::State protoState; proto->GetState( protoState ); ICustomBody::State deuterState; deuter->GetState( deuterState ); - Float4 protoG = protoState.GetLinearMomentum( worldPointOfContact ), - deuterG = deuterState.GetLinearMomentum( worldPointOfContact ); + Float4 protoG = protoState.GetLinearMomentum( worldPointOfContact.xyz ), + deuterG = deuterState.GetLinearMomentum( worldPointOfContact.xyz ); // calc from perspective of deuter Float4 normal; deuter->GetNormalAt( worldPointOfContact, normal ); - Float protoG_Magnitude = protoG.Dot( normal ), + Float protoG_Magnitude = protoG.Dot( normal ), deuterG_Magnitude = deuterG.Dot( normal ); // if they are not relatively moving towards eachother, there is no collision - Float deltaPos = normal.Dot( deuterState.GetCenterPosition() - protoState.GetCenterPosition() ); + Float deltaPos = normal.Dot( Float4(deuterState.GetCenterPosition(), 1) - Float4(protoState.GetCenterPosition(), 1) ); if( deltaPos < 0.0f ) { if( protoG_Magnitude >= deuterG_Magnitude ) @@ -95,12 +95,15 @@ namespace // } + Float kineticEnergyPBefore = Oyster::Physics3D::Formula::LinearKineticEnergy( protoState.GetMass(), protoState.GetLinearMomentum()/protoState.GetMass() ); // protoState.ApplyForwarding( forwardedDeltaPos, forwardedDeltaAxis ); - protoState.ApplyImpulse( bounce, worldPointOfContact, normal ); + protoState.ApplyImpulse( bounce.xyz, worldPointOfContact.xyz, normal.xyz ); proto->SetState( protoState ); - proto->CallSubscription_CollisionResponse( deuter, protoState.GetLinearMomentum().GetMagnitude()/(protoState.GetMass() + protoState.GetLinearMomentum().GetMagnitude())); + Float kineticEnergyPAFter = Oyster::Physics3D::Formula::LinearKineticEnergy( protoState.GetMass(), (protoState.GetLinearMomentum() + protoState.GetLinearImpulse())/protoState.GetMass() ); + + proto->CallSubscription_CollisionResponse( deuter, kineticEnergyPBefore - kineticEnergyPAFter ); } break; @@ -176,7 +179,7 @@ void API_Impl::Update() { case Gravity::GravityType_Well: { - Float4 d = Float4( this->gravity[i].well.position, 1.0f ) - state.GetCenterPosition(); + Float4 d = Float4( this->gravity[i].well.position, 1.0f ) - Float4( state.GetCenterPosition(), 1.0f ); Float rSquared = d.Dot( d ); if( rSquared != 0.0 ) { @@ -198,11 +201,9 @@ void API_Impl::Update() if( gravityImpulse != Float4::null ) { - state.ApplyLinearImpulse( gravityImpulse ); - //state.SetGravityNormal( gravityImpulse.GetNormalized()); - //(*proto)->SetGravityNormal( gravityImpulse.GetNormalized().xyz ); - (*proto)->SetState( state ); + state.ApplyLinearImpulse( gravityImpulse.xyz ); (*proto)->SetGravityNormal( gravityImpulse.GetNormalized().xyz ); + (*proto)->SetState( state ); } // Step 2: Apply Collision Response @@ -275,9 +276,9 @@ void API_Impl::RemoveGravity( const API::Gravity &g ) } } -void API_Impl::ApplyEffect( const Oyster::Collision3D::ICollideable& collideable, void(hitAction)(ICustomBody*) ) +void API_Impl::ApplyEffect( const Oyster::Collision3D::ICollideable& collideable, void* args, void(hitAction)(ICustomBody*, void*) ) { - this->worldScene.Visit(collideable, hitAction); + this->worldScene.Visit(collideable, args, hitAction); } //void API_Impl::ApplyForceAt( const ICustomBody* objRef, const Float3 &worldPos, const Float3 &worldF ) diff --git a/Code/GamePhysics/Implementation/PhysicsAPI_Impl.h b/Code/GamePhysics/Implementation/PhysicsAPI_Impl.h index b9343ae6..63d8ff08 100644 --- a/Code/GamePhysics/Implementation/PhysicsAPI_Impl.h +++ b/Code/GamePhysics/Implementation/PhysicsAPI_Impl.h @@ -35,7 +35,7 @@ namespace Oyster void AddGravity( const API::Gravity &g ); void RemoveGravity( const API::Gravity &g ); - void ApplyEffect( const Oyster::Collision3D::ICollideable& collideable, void(hitAction)(ICustomBody*) ); + void ApplyEffect( const Oyster::Collision3D::ICollideable& collideable, void* args, void(hitAction)(ICustomBody*, void*) ); //void ApplyForceAt( const ICustomBody* objRef, const ::Oyster::Math::Float3 &worldPos, const ::Oyster::Math::Float3 &worldF ); diff --git a/Code/GamePhysics/Implementation/SimpleRigidBody.cpp b/Code/GamePhysics/Implementation/SimpleRigidBody.cpp index 5a9bf53f..2f6a6c0c 100644 --- a/Code/GamePhysics/Implementation/SimpleRigidBody.cpp +++ b/Code/GamePhysics/Implementation/SimpleRigidBody.cpp @@ -56,7 +56,7 @@ SimpleRigidBody::SimpleRigidBody() SimpleRigidBody::SimpleRigidBody( const API::SimpleBodyDescription &desc ) { - this->rigid.SetRotation( desc.rotation ); + //this->rigid.SetRotation( desc.rotation ); this->rigid.centerPos = desc.centerPosition; this->rigid.SetSize( desc.size ); this->rigid.SetMass_KeepMomentum( desc.mass ); @@ -143,8 +143,8 @@ void SimpleRigidBody::SetState( const SimpleRigidBody::State &state ) if( state.IsForwarded() ) { - this->deltaPos += state.GetForward_DeltaPos(); - this->deltaAxis += state.GetForward_DeltaAxis(); + this->deltaPos += Float4(state.GetForward_DeltaPos(), 0); + this->deltaAxis += Float4(state.GetForward_DeltaAxis(), 0); this->isForwarded; } @@ -205,7 +205,7 @@ Sphere & SimpleRigidBody::GetBoundingSphere( Sphere &targetMem ) const Float4 & SimpleRigidBody::GetNormalAt( const Float4 &worldPos, Float4 &targetMem ) const { - Float4 offset = worldPos - this->rigid.centerPos; + Float4 offset = worldPos.xyz - this->rigid.centerPos; Float distance = offset.Dot( offset ); Float3 normal = Float3::null; @@ -295,7 +295,7 @@ UpdateState SimpleRigidBody::Update( Float timeStepLength ) { if( this->isForwarded ) { - this->rigid.Move( this->deltaPos, this->deltaAxis ); + this->rigid.Move( this->deltaPos.xyz, this->deltaAxis.xyz ); this->deltaPos = Float4::null; this->deltaAxis = Float4::null; this->isForwarded = false; @@ -310,7 +310,7 @@ UpdateState SimpleRigidBody::Update( Float timeStepLength ) void SimpleRigidBody::Predict( Float4 &outDeltaPos, Float4 &outDeltaAxis, const Float4 &actingLinearImpulse, const Float4 &actingAngularImpulse, Float deltaTime ) { - this->rigid.Predict_LeapFrog( outDeltaPos, outDeltaAxis, actingLinearImpulse, actingAngularImpulse, deltaTime ); + this->rigid.Predict_LeapFrog( outDeltaPos.xyz, outDeltaAxis.xyz, actingLinearImpulse.xyz, actingAngularImpulse.xyz, deltaTime ); } void SimpleRigidBody::SetScene( void *scene ) diff --git a/Code/GamePhysics/Implementation/SphericalRigidBody.cpp b/Code/GamePhysics/Implementation/SphericalRigidBody.cpp index 9fd8d219..a164cbe1 100644 --- a/Code/GamePhysics/Implementation/SphericalRigidBody.cpp +++ b/Code/GamePhysics/Implementation/SphericalRigidBody.cpp @@ -24,11 +24,11 @@ SphericalRigidBody::SphericalRigidBody() SphericalRigidBody::SphericalRigidBody( const API::SphericalBodyDescription &desc ) { this->rigid = RigidBody(); - this->rigid.SetRotation( desc.rotation ); + //this->rigid.SetRotation( desc.rotation ); this->rigid.centerPos = desc.centerPosition; this->rigid.boundingReach = Float4( desc.radius, desc.radius, desc.radius, 0.0f ); this->rigid.SetMass_KeepMomentum( desc.mass ); - this->rigid.SetMomentOfInertia_KeepMomentum( Formula::MomentOfInertia::CreateSphereMatrix( desc.mass, desc.radius ) ); + this->rigid.SetMomentOfInertia_KeepMomentum( MomentOfInertia::Sphere(desc.mass, desc.radius) ); this->deltaPos = Float4::null; this->deltaAxis = Float4::null; @@ -108,8 +108,8 @@ void SphericalRigidBody::SetState( const SphericalRigidBody::State &state ) if( state.IsForwarded() ) { - this->deltaPos += state.GetForward_DeltaPos(); - this->deltaAxis += state.GetForward_DeltaAxis(); + this->deltaPos += Float4(state.GetForward_DeltaPos(), 0); + this->deltaAxis += Float4(state.GetForward_DeltaAxis()); this->isForwarded = false; } @@ -171,7 +171,7 @@ Sphere & SphericalRigidBody::GetBoundingSphere( Sphere &targetMem ) const Float4 & SphericalRigidBody::GetNormalAt( const Float4 &worldPos, Float4 &targetMem ) const { - targetMem = worldPos - this->rigid.centerPos; + targetMem = worldPos.xyz - this->rigid.centerPos; Float magnitude = targetMem.GetMagnitude(); if( magnitude != 0.0f ) { // sanity check @@ -220,7 +220,7 @@ UpdateState SphericalRigidBody::Update( Float timeStepLength ) { if( this->isForwarded ) { - this->rigid.Move( this->deltaPos, this->deltaAxis ); + this->rigid.Move( this->deltaPos.xyz, this->deltaAxis.xyz ); this->deltaPos = Float4::null; this->deltaAxis = Float4::null; this->isForwarded = false; @@ -235,7 +235,7 @@ UpdateState SphericalRigidBody::Update( Float timeStepLength ) void SphericalRigidBody::Predict( ::Oyster::Math::Float4 &outDeltaPos, ::Oyster::Math::Float4 &outDeltaAxis, const ::Oyster::Math::Float4 &actingLinearImpulse, const ::Oyster::Math::Float4 &actingAngularImpulse, ::Oyster::Math::Float deltaTime ) { - this->rigid.Predict_LeapFrog( outDeltaPos, outDeltaAxis, actingLinearImpulse, actingAngularImpulse, deltaTime ); + this->rigid.Predict_LeapFrog( outDeltaPos.xyz, outDeltaAxis.xyz, actingLinearImpulse.xyz, actingAngularImpulse.xyz, deltaTime ); } void SphericalRigidBody::SetSubscription( ICustomBody::EventAction_Collision functionPointer ) diff --git a/Code/GamePhysics/PhysicsAPI.h b/Code/GamePhysics/PhysicsAPI.h index 7f20fcf7..3ced2a55 100644 --- a/Code/GamePhysics/PhysicsAPI.h +++ b/Code/GamePhysics/PhysicsAPI.h @@ -139,9 +139,11 @@ namespace Oyster /******************************************************** * Applies an effect to objects that collide with the set volume. * @param collideable: An ICollideable that defines the volume of the effect. - * @param hitAction: A function that contains the effect. + * @param args: The arguments needed for the hitAction function. + * @param hitAction: A function that contains the effect. Parameterlist contains the custom body + the collideable hits, and the arguments sent to the function. ********************************************************/ - virtual void ApplyEffect( const Oyster::Collision3D::ICollideable& collideable, void(hitAction)(ICustomBody*) ) = 0; + virtual void ApplyEffect( const Oyster::Collision3D::ICollideable& collideable, void* args, void(hitAction)(ICustomBody*, void*) ) = 0; ///******************************************************** // * Apply force on an object. diff --git a/Code/GamePhysics/PhysicsStructs-Impl.h b/Code/GamePhysics/PhysicsStructs-Impl.h index 46de90cc..fa5533fa 100644 --- a/Code/GamePhysics/PhysicsStructs-Impl.h +++ b/Code/GamePhysics/PhysicsStructs-Impl.h @@ -13,13 +13,13 @@ namespace Oyster inline SimpleBodyDescription::SimpleBodyDescription() { this->rotation = ::Oyster::Math::Float4x4::identity; - this->centerPosition = ::Oyster::Math::Float4::standard_unit_w; - this->size = ::Oyster::Math::Float4( 1.0f ); + this->centerPosition = ::Oyster::Math::Float3::null; + this->size = ::Oyster::Math::Float3( 1.0f ); this->mass = 12.0f; this->restitutionCoeff = 1.0f; this->frictionCoeff_Dynamic = 0.5f; this->frictionCoeff_Static = 0.5f; - this->inertiaTensor = ::Oyster::Math::Float4x4::identity; + this->inertiaTensor = ::Oyster::Physics3D::MomentOfInertia(); this->subscription_onCollision = NULL; this->subscription_onCollisionResponse = NULL; this->subscription_onMovement = NULL; @@ -29,7 +29,7 @@ namespace Oyster inline SphericalBodyDescription::SphericalBodyDescription() { this->rotation = ::Oyster::Math::Float4x4::identity; - this->centerPosition = ::Oyster::Math::Float4::standard_unit_w; + this->centerPosition = ::Oyster::Math::Float3::null; this->radius = 0.5f; this->mass = 10.0f; this->restitutionCoeff = 1.0f; @@ -41,7 +41,7 @@ namespace Oyster this->ignoreGravity = false; } - inline CustomBodyState::CustomBodyState( ::Oyster::Math::Float mass, ::Oyster::Math::Float restitutionCoeff, ::Oyster::Math::Float staticFrictionCoeff, ::Oyster::Math::Float kineticFrictionCoeff, const ::Oyster::Math::Float4x4 &inertiaTensor, const ::Oyster::Math::Float4 &reach, const ::Oyster::Math::Float4 ¢erPos, const ::Oyster::Math::Float4 &rotation, const ::Oyster::Math::Float4 &linearMomentum, const ::Oyster::Math::Float4 &angularMomentum, const ::Oyster::Math::Float4 &gravityNormal ) + inline CustomBodyState::CustomBodyState( ::Oyster::Math::Float mass, ::Oyster::Math::Float restitutionCoeff, ::Oyster::Math::Float staticFrictionCoeff, ::Oyster::Math::Float kineticFrictionCoeff, const ::Oyster::Physics3D::MomentOfInertia &inertiaTensor, const ::Oyster::Math::Float3 &reach, const ::Oyster::Math::Float3 ¢erPos, const ::Oyster::Math::Float3 &rotation, const ::Oyster::Math::Float3 &linearMomentum, const ::Oyster::Math::Float3 &angularMomentum, const ::Oyster::Math::Float3 &gravityNormal ) { this->mass = mass; this->restitutionCoeff = restitutionCoeff; @@ -53,8 +53,8 @@ namespace Oyster this->angularAxis = rotation; this->linearMomentum = linearMomentum; this->angularMomentum = angularMomentum; - this->linearImpulse = this->angularImpulse = ::Oyster::Math::Float4::null; - this->deltaPos = this->deltaAxis = ::Oyster::Math::Float4::null; + this->linearImpulse = this->angularImpulse = ::Oyster::Math::Float3::null; + this->deltaPos = this->deltaAxis = ::Oyster::Math::Float3::null; this->isSpatiallyAltered = this->isDisturbed = this->isForwarded = false; this->gravityNormal = gravityNormal; } @@ -102,92 +102,92 @@ namespace Oyster return this->kineticFrictionCoeff; } - inline const ::Oyster::Math::Float4x4 & CustomBodyState::GetMomentOfInertia() const + inline const ::Oyster::Physics3D::MomentOfInertia & CustomBodyState::GetMomentOfInertia() const { return this->inertiaTensor; } - inline const ::Oyster::Math::Float4 & CustomBodyState::GetReach() const + inline const ::Oyster::Math::Float3 & CustomBodyState::GetReach() const { return this->reach; } - inline ::Oyster::Math::Float4 CustomBodyState::GetSize() const + inline ::Oyster::Math::Float3 CustomBodyState::GetSize() const { return 2.0f * this->GetReach(); } - inline const ::Oyster::Math::Float4 & CustomBodyState::GetCenterPosition() const + inline const ::Oyster::Math::Float3 & CustomBodyState::GetCenterPosition() const { return this->centerPos; } - inline const ::Oyster::Math::Float4 & CustomBodyState::GetAngularAxis() const + inline const ::Oyster::Math::Float3 & CustomBodyState::GetAngularAxis() const { return this->angularAxis; } inline ::Oyster::Math::Float4x4 CustomBodyState::GetRotation() const { - return ::Oyster::Math3D::RotationMatrix( this->GetAngularAxis().xyz ); + return ::Oyster::Math3D::RotationMatrix( this->GetAngularAxis() ); } inline ::Oyster::Math::Float4x4 CustomBodyState::GetOrientation() const { - return ::Oyster::Math3D::OrientationMatrix( this->angularAxis.xyz, this->centerPos.xyz ); + return ::Oyster::Math3D::OrientationMatrix( this->angularAxis, this->centerPos ); } - inline ::Oyster::Math::Float4x4 CustomBodyState::GetOrientation( const ::Oyster::Math::Float4 &offset ) const + inline ::Oyster::Math::Float4x4 CustomBodyState::GetOrientation( const ::Oyster::Math::Float3 &offset ) const { - return ::Oyster::Math3D::OrientationMatrix( this->angularAxis.xyz, (this->centerPos + offset).xyz ); + return ::Oyster::Math3D::OrientationMatrix( this->angularAxis, (this->centerPos + offset) ); } inline ::Oyster::Math::Float4x4 CustomBodyState::GetView() const { - return ::Oyster::Math3D::ViewMatrix( this->angularAxis.xyz, this->centerPos.xyz ); + return ::Oyster::Math3D::ViewMatrix( this->angularAxis, this->centerPos ); } - inline ::Oyster::Math::Float4x4 CustomBodyState::GetView( const ::Oyster::Math::Float4 &offset ) const + inline ::Oyster::Math::Float4x4 CustomBodyState::GetView( const ::Oyster::Math::Float3 &offset ) const { - return ::Oyster::Math3D::ViewMatrix( this->angularAxis.xyz, (this->centerPos + offset).xyz ); + return ::Oyster::Math3D::ViewMatrix( this->angularAxis, (this->centerPos + offset) ); } - inline const ::Oyster::Math::Float4 & CustomBodyState::GetLinearMomentum() const + inline const ::Oyster::Math::Float3 & CustomBodyState::GetLinearMomentum() const { return this->linearMomentum; } - inline ::Oyster::Math::Float4 CustomBodyState::GetLinearMomentum( const ::Oyster::Math::Float4 &at ) const + inline ::Oyster::Math::Float3 CustomBodyState::GetLinearMomentum( const ::Oyster::Math::Float3 &at ) const { return this->linearMomentum + ::Oyster::Physics3D::Formula::TangentialLinearMomentum( this->angularMomentum, at - this->centerPos ); } - inline const ::Oyster::Math::Float4 & CustomBodyState::GetAngularMomentum() const + inline const ::Oyster::Math::Float3 & CustomBodyState::GetAngularMomentum() const { return this->angularMomentum; } - inline const ::Oyster::Math::Float4 & CustomBodyState::GetLinearImpulse() const + inline const ::Oyster::Math::Float3 & CustomBodyState::GetLinearImpulse() const { return this->linearImpulse; } - inline const ::Oyster::Math::Float4 & CustomBodyState::GetAngularImpulse() const + inline const ::Oyster::Math::Float3 & CustomBodyState::GetAngularImpulse() const { return this->angularImpulse; } - inline const ::Oyster::Math::Float4 & CustomBodyState::GetForward_DeltaPos() const + inline const ::Oyster::Math::Float3 & CustomBodyState::GetForward_DeltaPos() const { return this->deltaPos; } - inline const ::Oyster::Math::Float4 & CustomBodyState::GetForward_DeltaAxis() const + inline const ::Oyster::Math::Float3 & CustomBodyState::GetForward_DeltaAxis() const { return this->deltaAxis; } - inline const ::Oyster::Math::Float4 & CustomBodyState::GetGravityNormal() const + inline const ::Oyster::Math::Float3 & CustomBodyState::GetGravityNormal() const { return this->gravityNormal; } @@ -219,47 +219,51 @@ namespace Oyster this->kineticFrictionCoeff = kineticU; } - inline void CustomBodyState::SetMomentOfInertia_KeepMomentum( const ::Oyster::Math::Float4x4 &tensor ) + inline void CustomBodyState::SetMomentOfInertia_KeepMomentum( const ::Oyster::Physics3D::MomentOfInertia &tensor ) { this->inertiaTensor = tensor; } - inline void CustomBodyState::SetMomentOfInertia_KeepVelocity( const ::Oyster::Math::Float4x4 &tensor ) + inline void CustomBodyState::SetMomentOfInertia_KeepVelocity( const ::Oyster::Physics3D::MomentOfInertia &tensor ) { - if( tensor.GetDeterminant() != 0.0f ) - { // sanity block! - ::Oyster::Math::Float4x4 rotation = ::Oyster::Math3D::RotationMatrix(this->angularAxis.xyz); - ::Oyster::Math::Float4 w = ::Oyster::Physics3D::Formula::AngularVelocity( (rotation * this->inertiaTensor).GetInverse(), this->angularMomentum ); - this->inertiaTensor = tensor; - this->angularMomentum = ::Oyster::Physics3D::Formula::AngularMomentum( rotation * tensor, w ); - } + ::Oyster::Math::Quaternion rotation = ::Oyster::Math3D::Rotation(this->angularAxis); + ::Oyster::Math::Float3 w = this->inertiaTensor.CalculateAngularVelocity( rotation, this->angularMomentum ); + this->inertiaTensor = tensor; + this->angularMomentum = this->inertiaTensor.CalculateAngularMomentum( rotation, w ); } - inline void CustomBodyState::SetSize( const ::Oyster::Math::Float4 &size ) + inline void CustomBodyState::SetSize( const ::Oyster::Math::Float3 &size ) { this->SetReach( 0.5f * size ); } - inline void CustomBodyState::SetReach( const ::Oyster::Math::Float4 &halfSize ) + inline void CustomBodyState::SetReach( const ::Oyster::Math::Float3 &halfSize ) { - this->reach.xyz = halfSize; - this->reach = ::Utility::Value::Max( this->reach, ::Oyster::Math::Float4::null ); + this->reach = halfSize; + this->reach = ::Utility::Value::Max( this->reach, ::Oyster::Math::Float3::null ); this->isSpatiallyAltered = this->isDisturbed = true; } - inline void CustomBodyState::SetCenterPosition( const ::Oyster::Math::Float4 ¢erPos ) + inline void CustomBodyState::SetCenterPosition( const ::Oyster::Math::Float3 ¢erPos ) { - this->centerPos.xyz = centerPos; + this->centerPos = centerPos; this->isSpatiallyAltered = this->isDisturbed = true; } - inline void CustomBodyState::SetRotation( const ::Oyster::Math::Float4 &angularAxis ) + inline void CustomBodyState::SetRotation( const ::Oyster::Math::Float3 &angularAxis ) { - this->angularAxis.xyz = angularAxis; + this->angularAxis = angularAxis; this->isSpatiallyAltered = this->isDisturbed = true; } - inline void CustomBodyState::SetRotation( const ::Oyster::Math::Float4x4 &rotation ) + inline void CustomBodyState::SetOrientation( const ::Oyster::Math::Float3 &angularAxis, const ::Oyster::Math::Float3 &translation ) + { + this->angularAxis = angularAxis ; + this->centerPos = translation; + this->isSpatiallyAltered = this->isDisturbed = true; + } + + /*inline void CustomBodyState::SetRotation( const ::Oyster::Math::Float4x4 &rotation ) { this->SetRotation( ::Oyster::Math3D::AngularAxis(rotation) ); } @@ -268,72 +272,74 @@ namespace Oyster { this->SetRotation( ::Oyster::Math3D::ExtractAngularAxis(orientation) ); this->SetCenterPosition( orientation.v[3] ); - } + }*/ - inline void CustomBodyState::SetLinearMomentum( const ::Oyster::Math::Float4 &g ) + + + inline void CustomBodyState::SetLinearMomentum( const ::Oyster::Math::Float3 &g ) { - this->linearMomentum.xyz = g; + this->linearMomentum = g; this->isDisturbed = true; } - inline void CustomBodyState::SetAngularMomentum( const ::Oyster::Math::Float4 &h ) + inline void CustomBodyState::SetAngularMomentum( const ::Oyster::Math::Float3 &h ) { - this->angularMomentum.xyz = h; + this->angularMomentum = h; this->isDisturbed = true; } - inline void CustomBodyState::SetLinearImpulse( const ::Oyster::Math::Float4 &j ) + inline void CustomBodyState::SetLinearImpulse( const ::Oyster::Math::Float3 &j ) { - this->linearImpulse.xyz = j; + this->linearImpulse = j; this->isDisturbed = true; } - inline void CustomBodyState::SetAngularImpulse( const ::Oyster::Math::Float4 &j ) + inline void CustomBodyState::SetAngularImpulse( const ::Oyster::Math::Float3 &j ) { - this->angularImpulse.xyz = j; + this->angularImpulse = j; this->isDisturbed = true; } - inline void CustomBodyState::SetGravityNormal( const ::Oyster::Math::Float4 &gravityNormal ) + inline void CustomBodyState::SetGravityNormal( const ::Oyster::Math::Float3 &gravityNormal ) { this->gravityNormal = gravityNormal; } - inline void CustomBodyState::AddRotation( const ::Oyster::Math::Float4 &angularAxis ) + inline void CustomBodyState::AddRotation( const ::Oyster::Math::Float3 &angularAxis ) { this->angularAxis += angularAxis; this->isSpatiallyAltered = this->isDisturbed = true; } - inline void CustomBodyState::AddTranslation( const ::Oyster::Math::Float4 &deltaPos ) + inline void CustomBodyState::AddTranslation( const ::Oyster::Math::Float3 &deltaPos ) { this->centerPos += deltaPos; this->isSpatiallyAltered = this->isDisturbed = true; } - inline void CustomBodyState::ApplyLinearImpulse( const ::Oyster::Math::Float4 &j ) + inline void CustomBodyState::ApplyLinearImpulse( const ::Oyster::Math::Float3 &j ) { this->linearImpulse += j; this->isDisturbed = true; } - inline void CustomBodyState::ApplyAngularImpulse( const ::Oyster::Math::Float4 &j ) + inline void CustomBodyState::ApplyAngularImpulse( const ::Oyster::Math::Float3 &j ) { this->angularImpulse += j; this->isDisturbed = true; } - inline void CustomBodyState::ApplyImpulse( const ::Oyster::Math::Float4 &j, const ::Oyster::Math::Float4 &at, const ::Oyster::Math::Float4 &normal ) + inline void CustomBodyState::ApplyImpulse( const ::Oyster::Math::Float3 &j, const ::Oyster::Math::Float3 &at, const ::Oyster::Math::Float3 &normal ) { - ::Oyster::Math::Float4 offset = at - this->centerPos; - ::Oyster::Math::Float4 deltaAngularImpulse = ::Oyster::Physics3D::Formula::AngularMomentum( j, offset ); + ::Oyster::Math::Float3 offset = at - this->centerPos; + ::Oyster::Math::Float3 deltaAngularImpulse = ::Oyster::Physics3D::Formula::AngularMomentum( j, offset ); this->linearImpulse += j - ::Oyster::Physics3D::Formula::TangentialLinearMomentum( deltaAngularImpulse, offset ); this->angularImpulse += deltaAngularImpulse; this->isDisturbed = true; } - inline void CustomBodyState::ApplyForwarding( const ::Oyster::Math::Float4 &deltaPos, const ::Oyster::Math::Float4 &deltaAxis ) + inline void CustomBodyState::ApplyForwarding( const ::Oyster::Math::Float3 &deltaPos, const ::Oyster::Math::Float3 &deltaAxis ) { this->deltaPos += deltaPos; this->deltaAxis += deltaAxis; diff --git a/Code/GamePhysics/PhysicsStructs.h b/Code/GamePhysics/PhysicsStructs.h index 1bc1736f..49cf6993 100644 --- a/Code/GamePhysics/PhysicsStructs.h +++ b/Code/GamePhysics/PhysicsStructs.h @@ -3,6 +3,7 @@ #include "OysterMath.h" #include "PhysicsAPI.h" +#include "Inertia.h" namespace Oyster { namespace Physics { @@ -11,13 +12,13 @@ namespace Oyster { namespace Physics struct SimpleBodyDescription { ::Oyster::Math::Float4x4 rotation; - ::Oyster::Math::Float4 centerPosition; - ::Oyster::Math::Float4 size; + ::Oyster::Math::Float3 centerPosition; + ::Oyster::Math::Float3 size; ::Oyster::Math::Float mass; ::Oyster::Math::Float restitutionCoeff; ::Oyster::Math::Float frictionCoeff_Static; ::Oyster::Math::Float frictionCoeff_Dynamic; - ::Oyster::Math::Float4x4 inertiaTensor; + ::Oyster::Physics3D::MomentOfInertia inertiaTensor; ::Oyster::Physics::ICustomBody::EventAction_Collision subscription_onCollision; ::Oyster::Physics::ICustomBody::EventAction_CollisionResponse subscription_onCollisionResponse; ::Oyster::Physics::ICustomBody::EventAction_Move subscription_onMovement; @@ -29,7 +30,7 @@ namespace Oyster { namespace Physics struct SphericalBodyDescription { ::Oyster::Math::Float4x4 rotation; - ::Oyster::Math::Float4 centerPosition; + ::Oyster::Math::Float3 centerPosition; ::Oyster::Math::Float radius; ::Oyster::Math::Float mass; ::Oyster::Math::Float restitutionCoeff; @@ -50,13 +51,13 @@ namespace Oyster { namespace Physics ::Oyster::Math::Float restitutionCoeff = 1.0f, ::Oyster::Math::Float staticFrictionCoeff = 1.0f, ::Oyster::Math::Float kineticFrictionCoeff = 1.0f, - const ::Oyster::Math::Float4x4 &inertiaTensor = ::Oyster::Math::Float4x4::identity, - const ::Oyster::Math::Float4 &reach = ::Oyster::Math::Float4::null, - const ::Oyster::Math::Float4 ¢erPos = ::Oyster::Math::Float4::standard_unit_w, - const ::Oyster::Math::Float4 &rotation = ::Oyster::Math::Float4::null, - const ::Oyster::Math::Float4 &linearMomentum = ::Oyster::Math::Float4::null, - const ::Oyster::Math::Float4 &angularMomentum = ::Oyster::Math::Float4::null, - const ::Oyster::Math::Float4 &gravityNormal = ::Oyster::Math::Float4::null); + const ::Oyster::Physics3D::MomentOfInertia &inertiaTensor = ::Oyster::Physics3D::MomentOfInertia(), + const ::Oyster::Math::Float3 &reach = ::Oyster::Math::Float3::null, + const ::Oyster::Math::Float3 ¢erPos = ::Oyster::Math::Float3::null, + const ::Oyster::Math::Float3 &rotation = ::Oyster::Math::Float3::null, + const ::Oyster::Math::Float3 &linearMomentum = ::Oyster::Math::Float3::null, + const ::Oyster::Math::Float3 &angularMomentum = ::Oyster::Math::Float3::null, + const ::Oyster::Math::Float3 &gravityNormal = ::Oyster::Math::Float3::null); CustomBodyState & operator = ( const CustomBodyState &state ); @@ -64,50 +65,51 @@ namespace Oyster { namespace Physics const ::Oyster::Math::Float GetRestitutionCoeff() const; const ::Oyster::Math::Float GetFrictionCoeff_Static() const; const ::Oyster::Math::Float GetFrictionCoeff_Kinetic() const; - const ::Oyster::Math::Float4x4 & GetMomentOfInertia() const; - const ::Oyster::Math::Float4 & GetReach() const; - ::Oyster::Math::Float4 GetSize() const; - const ::Oyster::Math::Float4 & GetCenterPosition() const; - const ::Oyster::Math::Float4 & GetAngularAxis() const; + const ::Oyster::Physics3D::MomentOfInertia & GetMomentOfInertia() const; + const ::Oyster::Math::Float3 & GetReach() const; + ::Oyster::Math::Float3 GetSize() const; + const ::Oyster::Math::Float3 & GetCenterPosition() const; + const ::Oyster::Math::Float3 & GetAngularAxis() const; ::Oyster::Math::Float4x4 GetRotation() const; ::Oyster::Math::Float4x4 GetOrientation() const; - ::Oyster::Math::Float4x4 GetOrientation( const ::Oyster::Math::Float4 &offset ) const; + ::Oyster::Math::Float4x4 GetOrientation( const ::Oyster::Math::Float3 &offset ) const; ::Oyster::Math::Float4x4 GetView() const; - ::Oyster::Math::Float4x4 GetView( const ::Oyster::Math::Float4 &offset ) const; - const ::Oyster::Math::Float4 & GetLinearMomentum() const; - ::Oyster::Math::Float4 GetLinearMomentum( const ::Oyster::Math::Float4 &at ) const; - const ::Oyster::Math::Float4 & GetAngularMomentum() const; - const ::Oyster::Math::Float4 & GetLinearImpulse() const; - const ::Oyster::Math::Float4 & GetAngularImpulse() const; - const ::Oyster::Math::Float4 & GetForward_DeltaPos() const; - const ::Oyster::Math::Float4 & GetForward_DeltaAxis() const; - const ::Oyster::Math::Float4 & GetGravityNormal() const; + ::Oyster::Math::Float4x4 GetView( const ::Oyster::Math::Float3 &offset ) const; + const ::Oyster::Math::Float3 & GetLinearMomentum() const; + ::Oyster::Math::Float3 GetLinearMomentum( const ::Oyster::Math::Float3 &at ) const; + const ::Oyster::Math::Float3 & GetAngularMomentum() const; + const ::Oyster::Math::Float3 & GetLinearImpulse() const; + const ::Oyster::Math::Float3 & GetAngularImpulse() const; + const ::Oyster::Math::Float3 & GetForward_DeltaPos() const; + const ::Oyster::Math::Float3 & GetForward_DeltaAxis() const; + const ::Oyster::Math::Float3 & GetGravityNormal() const; void SetMass_KeepMomentum( ::Oyster::Math::Float m ); void SetMass_KeepVelocity( ::Oyster::Math::Float m ); void SetRestitutionCoeff( ::Oyster::Math::Float e ); void SetFrictionCoeff( ::Oyster::Math::Float staticU, ::Oyster::Math::Float kineticU ); - void SetMomentOfInertia_KeepMomentum( const ::Oyster::Math::Float4x4 &tensor ); - void SetMomentOfInertia_KeepVelocity( const ::Oyster::Math::Float4x4 &tensor ); - void SetSize( const ::Oyster::Math::Float4 &size ); - void SetReach( const ::Oyster::Math::Float4 &halfSize ); - void SetCenterPosition( const ::Oyster::Math::Float4 ¢erPos ); - void SetRotation( const ::Oyster::Math::Float4 &angularAxis ); - void SetRotation( const ::Oyster::Math::Float4x4 &rotation ); - void SetOrientation( const ::Oyster::Math::Float4x4 &orientation ); - void SetLinearMomentum( const ::Oyster::Math::Float4 &g ); - void SetAngularMomentum( const ::Oyster::Math::Float4 &h ); - void SetLinearImpulse( const ::Oyster::Math::Float4 &j ); - void SetAngularImpulse( const ::Oyster::Math::Float4 &j ); - void SetGravityNormal( const ::Oyster::Math::Float4 &gravityNormal ); + void SetMomentOfInertia_KeepMomentum( const ::Oyster::Physics3D::MomentOfInertia &tensor ); + void SetMomentOfInertia_KeepVelocity( const ::Oyster::Physics3D::MomentOfInertia &tensor ); + void SetSize( const ::Oyster::Math::Float3 &size ); + void SetReach( const ::Oyster::Math::Float3 &halfSize ); + void SetCenterPosition( const ::Oyster::Math::Float3 ¢erPos ); + void SetRotation( const ::Oyster::Math::Float3 &angularAxis ); + //void SetRotation( const ::Oyster::Math::Float4x4 &rotation ); + //void SetOrientation( const ::Oyster::Math::Float4x4 &orientation ); + void SetOrientation( const ::Oyster::Math::Float3 &angularAxis, const ::Oyster::Math::Float3 &translation ); + void SetLinearMomentum( const ::Oyster::Math::Float3 &g ); + void SetAngularMomentum( const ::Oyster::Math::Float3 &h ); + void SetLinearImpulse( const ::Oyster::Math::Float3 &j ); + void SetAngularImpulse( const ::Oyster::Math::Float3 &j ); + void SetGravityNormal( const ::Oyster::Math::Float3 &gravityNormal ); - void AddRotation( const ::Oyster::Math::Float4 &angularAxis ); - void AddTranslation( const ::Oyster::Math::Float4 &deltaPos ); + void AddRotation( const ::Oyster::Math::Float3 &angularAxis ); + void AddTranslation( const ::Oyster::Math::Float3 &deltaPos ); - void ApplyLinearImpulse( const ::Oyster::Math::Float4 &j ); - void ApplyAngularImpulse( const ::Oyster::Math::Float4 &j ); - void ApplyImpulse( const ::Oyster::Math::Float4 &j, const ::Oyster::Math::Float4 &at, const ::Oyster::Math::Float4 &normal ); - void ApplyForwarding( const ::Oyster::Math::Float4 &deltaPos, const ::Oyster::Math::Float4 &deltaAxis ); + void ApplyLinearImpulse( const ::Oyster::Math::Float3 &j ); + void ApplyAngularImpulse( const ::Oyster::Math::Float3 &j ); + void ApplyImpulse( const ::Oyster::Math::Float3 &j, const ::Oyster::Math::Float3 &at, const ::Oyster::Math::Float3 &normal ); + void ApplyForwarding( const ::Oyster::Math::Float3 &deltaPos, const ::Oyster::Math::Float3 &deltaAxis ); bool IsSpatiallyAltered() const; bool IsDisturbed() const; @@ -115,12 +117,12 @@ namespace Oyster { namespace Physics private: ::Oyster::Math::Float mass, restitutionCoeff, staticFrictionCoeff, kineticFrictionCoeff; - ::Oyster::Math::Float4x4 inertiaTensor; - ::Oyster::Math::Float4 reach, centerPos, angularAxis; - ::Oyster::Math::Float4 linearMomentum, angularMomentum; - ::Oyster::Math::Float4 linearImpulse, angularImpulse; - ::Oyster::Math::Float4 deltaPos, deltaAxis; // Forwarding data sum - ::Oyster::Math::Float4 gravityNormal; + ::Oyster::Physics3D::MomentOfInertia inertiaTensor; + ::Oyster::Math::Float3 reach, centerPos, angularAxis; + ::Oyster::Math::Float3 linearMomentum, angularMomentum; + ::Oyster::Math::Float3 linearImpulse, angularImpulse; + ::Oyster::Math::Float3 deltaPos, deltaAxis; // Forwarding data sum + ::Oyster::Math::Float3 gravityNormal; bool isSpatiallyAltered, isDisturbed, isForwarded; }; diff --git a/Code/OysterMath/LinearMath.h b/Code/OysterMath/LinearMath.h index d32ea04f..50bcd36b 100644 --- a/Code/OysterMath/LinearMath.h +++ b/Code/OysterMath/LinearMath.h @@ -35,7 +35,7 @@ namespace std // x2 template -::LinearAlgebra::Matrix2x2 operator * ( const ::LinearAlgebra::Matrix2x2 &left, const ::LinearAlgebra::Matrix2x2 &right ) +inline ::LinearAlgebra::Matrix2x2 operator * ( const ::LinearAlgebra::Matrix2x2 &left, const ::LinearAlgebra::Matrix2x2 &right ) { return ::LinearAlgebra::Matrix2x2( (left.m11 * right.m11) + (left.m12 * right.m21), (left.m11 * right.m12) + (left.m12 * right.m22), @@ -44,14 +44,14 @@ template } template -::LinearAlgebra::Vector2 operator * ( const ::LinearAlgebra::Matrix2x2 &matrix, const ::LinearAlgebra::Vector2 &vector ) +inline ::LinearAlgebra::Vector2 operator * ( const ::LinearAlgebra::Matrix2x2 &matrix, const ::LinearAlgebra::Vector2 &vector ) { return ::LinearAlgebra::Vector2( (matrix.m11 * vector.x) + (matrix.m12 * vector.y), (matrix.m21 * vector.x) + (matrix.m22 * vector.y) ); } template -::LinearAlgebra::Vector2 operator * ( const ::LinearAlgebra::Vector2 &vector, const ::LinearAlgebra::Matrix2x2 &left ) +inline ::LinearAlgebra::Vector2 operator * ( const ::LinearAlgebra::Vector2 &vector, const ::LinearAlgebra::Matrix2x2 &left ) { return ::LinearAlgebra::Vector2( (vector.x * matrix.m11) + (vector.y * matrix.m21), (vector.x * matrix.m12) + (vector.y * matrix.m22) ); @@ -60,7 +60,7 @@ template // x3 template -::LinearAlgebra::Matrix3x3 operator * ( const ::LinearAlgebra::Matrix3x3 &left, const ::LinearAlgebra::Matrix3x3 &right ) +inline ::LinearAlgebra::Matrix3x3 operator * ( const ::LinearAlgebra::Matrix3x3 &left, const ::LinearAlgebra::Matrix3x3 &right ) { return ::LinearAlgebra::Matrix3x3( (left.m11 * right.m11) + (left.m12 * right.m21) + (left.m13 * right.m31), (left.m11 * right.m12) + (left.m12 * right.m22) + (left.m13 * right.m32), (left.m11 * right.m13) + (left.m12 * right.m23) + (left.m13 * right.m33), (left.m21 * right.m11) + (left.m22 * right.m21) + (left.m23 * right.m31), (left.m21 * right.m12) + (left.m22 * right.m22) + (left.m23 * right.m32), (left.m21 * right.m13) + (left.m22 * right.m23) + (left.m23 * right.m33), @@ -68,7 +68,7 @@ template } template -::LinearAlgebra::Vector3 operator * ( const ::LinearAlgebra::Matrix3x3 &matrix, const ::LinearAlgebra::Vector3 &vector ) +inline ::LinearAlgebra::Vector3 operator * ( const ::LinearAlgebra::Matrix3x3 &matrix, const ::LinearAlgebra::Vector3 &vector ) { return ::LinearAlgebra::Vector3( (matrix.m11 * vector.x) + (matrix.m12 * vector.y) + (matrix.m13 * vector.z), (matrix.m21 * vector.x) + (matrix.m22 * vector.y) + (matrix.m23 * vector.z), @@ -76,7 +76,7 @@ template } template -::LinearAlgebra::Vector3 operator * ( const ::LinearAlgebra::Vector3 &vector, const ::LinearAlgebra::Matrix3x3 &left ) +inline ::LinearAlgebra::Vector3 operator * ( const ::LinearAlgebra::Vector3 &vector, const ::LinearAlgebra::Matrix3x3 &left ) { return ::LinearAlgebra::Vector3( (vector.x * matrix.m11) + (vector.y * matrix.m21) + (vector.z * matrix.m31), (vector.x * matrix.m12) + (vector.y * matrix.m22) + (vector.z * matrix.m32), @@ -86,7 +86,7 @@ template // x4 template -::LinearAlgebra::Matrix4x4 operator * ( const ::LinearAlgebra::Matrix4x4 &left, const ::LinearAlgebra::Matrix4x4 &right ) +inline ::LinearAlgebra::Matrix4x4 operator * ( const ::LinearAlgebra::Matrix4x4 &left, const ::LinearAlgebra::Matrix4x4 &right ) { return ::LinearAlgebra::Matrix4x4( (left.m11 * right.m11) + (left.m12 * right.m21) + (left.m13 * right.m31) + (left.m14 * right.m41), (left.m11 * right.m12) + (left.m12 * right.m22) + (left.m13 * right.m32) + (left.m14 * right.m42), (left.m11 * right.m13) + (left.m12 * right.m23) + (left.m13 * right.m33) + (left.m14 * right.m43), (left.m11 * right.m14) + (left.m12 * right.m24) + (left.m13 * right.m34) + (left.m14 * right.m44), (left.m21 * right.m11) + (left.m22 * right.m21) + (left.m23 * right.m31) + (left.m24 * right.m41), (left.m21 * right.m12) + (left.m22 * right.m22) + (left.m23 * right.m32) + (left.m24 * right.m42), (left.m21 * right.m13) + (left.m22 * right.m23) + (left.m23 * right.m33) + (left.m24 * right.m43), (left.m21 * right.m14) + (left.m22 * right.m24) + (left.m23 * right.m34) + (left.m24 * right.m44), @@ -95,7 +95,7 @@ template } template -::LinearAlgebra::Vector4 operator * ( const ::LinearAlgebra::Matrix4x4 &matrix, const ::LinearAlgebra::Vector4 &vector ) +inline ::LinearAlgebra::Vector4 operator * ( const ::LinearAlgebra::Matrix4x4 &matrix, const ::LinearAlgebra::Vector4 &vector ) { return ::LinearAlgebra::Vector4( (matrix.m11 * vector.x) + (matrix.m12 * vector.y) + (matrix.m13 * vector.z) + (matrix.m14 * vector.w), (matrix.m21 * vector.x) + (matrix.m22 * vector.y) + (matrix.m23 * vector.z) + (matrix.m24 * vector.w), @@ -104,7 +104,7 @@ template } template -::LinearAlgebra::Vector4 operator * ( const ::LinearAlgebra::Vector4 &vector, const ::LinearAlgebra::Matrix4x4 &matrix ) +inline ::LinearAlgebra::Vector4 operator * ( const ::LinearAlgebra::Vector4 &vector, const ::LinearAlgebra::Matrix4x4 &matrix ) { return ::LinearAlgebra::Vector4( (vector.x * matrix.m11) + (vector.y * matrix.m21) + (vector.z * matrix.m31) + (vector.w * matrix.m41), (vector.x * matrix.m12) + (vector.y * matrix.m22) + (vector.z * matrix.m32) + (vector.w * matrix.m42), @@ -204,6 +204,19 @@ namespace LinearAlgebra } return output; // error: returning nullvector } + + /******************************************************************** + * Spherical Linear Interpolation on Quaternions + ********************************************************************/ + template + inline Quaternion Slerp( const Quaternion &start, const Quaternion &end, const ScalarType &t ) + { + ScalarType angle = (ScalarType)::std::acos( Vector4(start.imaginary, start.real).Dot(Vector4(end.imaginary, end.real)) ); + Quaternion result = start * (ScalarType)::std::sin( angle * (1 - t) ); + result += end * (ScalarType)::std::sin( angle * t ); + result *= (ScalarType)1.0f / (ScalarType)::std::sin( angle ); + return result; + } } namespace LinearAlgebra2D @@ -312,23 +325,24 @@ namespace LinearAlgebra2D namespace LinearAlgebra3D { - template - inline ::LinearAlgebra::Vector4 AngularAxis( const ::LinearAlgebra::Matrix3x3 &rotationMatrix ) - { - return ::std::asin( ::LinearAlgebra::Vector4(rotationMatrix.v[1].z, rotationMatrix.v[2].x, rotationMatrix.v[0].y, 1) ); - } + // All Matrix to AngularAxis conversions here is incorrect + //template + //inline ::LinearAlgebra::Vector4 AngularAxis( const ::LinearAlgebra::Matrix3x3 &rotationMatrix ) + //{ + // return ::std::asin( ::LinearAlgebra::Vector4(rotationMatrix.v[1].z, rotationMatrix.v[2].x, rotationMatrix.v[0].y, 1) ); + //} - template - inline ::LinearAlgebra::Vector4 AngularAxis( const ::LinearAlgebra::Matrix4x4 &rotationMatrix ) - { - return ::std::asin( ::LinearAlgebra::Vector4(rotationMatrix.v[1].z, rotationMatrix.v[2].x, rotationMatrix.v[0].y, 1) ); - } + //template + //inline ::LinearAlgebra::Vector4 AngularAxis( const ::LinearAlgebra::Matrix4x4 &rotationMatrix ) + //{ + // return ::std::asin( ::LinearAlgebra::Vector4(rotationMatrix.v[1].z, rotationMatrix.v[2].x, rotationMatrix.v[0].y, 1) ); + //} - template - inline ::LinearAlgebra::Vector4 ExtractAngularAxis( const ::LinearAlgebra::Matrix4x4 &orientationMatrix ) - { - return ::std::asin( ::LinearAlgebra::Vector4(orientationMatrix.v[1].z, orientationMatrix.v[2].x, orientationMatrix.v[0].y, 0) ); - } + //template + //inline ::LinearAlgebra::Vector4 ExtractAngularAxis( const ::LinearAlgebra::Matrix4x4 &orientationMatrix ) + //{ + // return ::std::asin( ::LinearAlgebra::Vector4(orientationMatrix.v[1].z, orientationMatrix.v[2].x, orientationMatrix.v[0].y, 0) ); + //} template inline ::LinearAlgebra::Matrix4x4 & TranslationMatrix( const ::LinearAlgebra::Vector3 &position, ::LinearAlgebra::Matrix4x4 &targetMem = ::LinearAlgebra::Matrix4x4() ) @@ -756,6 +770,28 @@ namespace LinearAlgebra3D return rotation; // return no change return SnapAxisYToNormal_UsingNlerp( rotation, interpolated ); } + + template + ::LinearAlgebra::Matrix4x4 & InterpolateOrientation_UsingNonRigidNlerp( const ::LinearAlgebra::Matrix4x4 &start, const ::LinearAlgebra::Matrix4x4 &end, ScalarType t, ::LinearAlgebra::Matrix4x4 &targetMem ) + { + targetMem.v[0] = ::LinearAlgebra::Nlerp( start.v[0], end.v[0], t ); + targetMem.v[1] = ::LinearAlgebra::Nlerp( start.v[1], end.v[1], t ); + targetMem.v[2] = ::LinearAlgebra::Nlerp( start.v[2], end.v[2], t ); + targetMem.v[3] = ::LinearAlgebra::Lerp( start.v[3], end.v[3], t ); + return targetMem; + } + + template + ::LinearAlgebra::Matrix4x4 & InterpolateOrientation_UsingNonRigidNlerp( const ::LinearAlgebra::Quaternion &startR, const ::LinearAlgebra::Vector3 &startT, const ::LinearAlgebra::Quaternion &endR, const ::LinearAlgebra::Vector3 &endT, ScalarType t, ::LinearAlgebra::Matrix4x4 &targetMem ) + { + return InterpolateOrientation_UsingNonRigidNlerp( OrientationMatrix(startR, startT), OrientationMatrix(endR, endT), t, targetMem ); + } + + template + ::LinearAlgebra::Matrix4x4 & InterpolateOrientation_UsingSlerp( const ::LinearAlgebra::Quaternion &startR, const ::LinearAlgebra::Vector3 &startT, const ::LinearAlgebra::Quaternion &endR, const ::LinearAlgebra::Vector3 &endT, ScalarType t, ::LinearAlgebra::Matrix4x4 &targetMem ) + { + return OrientationMatrix( ::LinearAlgebra::Slerp(startR, endR, t), ::LinearAlgebra::Lerp(::LinearAlgebra::Vector4(startT, (ScalarType)1.0f), ::LinearAlgebra::Vector4(endT, (ScalarType)1.0f), t).xyz, targetMem ); + } } #include "Utilities.h" diff --git a/Code/OysterMath/Matrix.h b/Code/OysterMath/Matrix.h index c7b6b9e0..5d129431 100644 --- a/Code/OysterMath/Matrix.h +++ b/Code/OysterMath/Matrix.h @@ -163,12 +163,18 @@ namespace LinearAlgebra Vector4 GetRowVector( unsigned int rowID ) const; const Vector4 & GetColumnVector( unsigned int colID ) const; }; +} +template LinearAlgebra::Matrix2x2 operator * ( const ScalarType &left, const LinearAlgebra::Matrix2x2 &right ); +template LinearAlgebra::Matrix3x3 operator * ( const ScalarType &left, const LinearAlgebra::Matrix3x3 &right ); +template LinearAlgebra::Matrix4x4 operator * ( const ScalarType &left, const LinearAlgebra::Matrix4x4 &right ); /////////////////////////////////////////////////////////////////////////////////// // Body /////////////////////////////////////////////////////////////////////////////////// +namespace LinearAlgebra +{ // Matrix2x2 /////////////////////////////////////// template @@ -753,4 +759,22 @@ namespace LinearAlgebra { return this->v[colID]; } } +template +inline LinearAlgebra::Matrix2x2 operator * ( const ScalarType &left, const LinearAlgebra::Matrix2x2 &right ) +{ + return right * left; +} + +template +inline LinearAlgebra::Matrix3x3 operator * ( const ScalarType &left, const LinearAlgebra::Matrix3x3 &right ) +{ + return right * left; +} + +template +inline LinearAlgebra::Matrix4x4 operator * ( const ScalarType &left, const LinearAlgebra::Matrix4x4 &right ) +{ + return right * left; +} + #endif \ No newline at end of file diff --git a/Code/OysterMath/OysterMath.cpp b/Code/OysterMath/OysterMath.cpp index f44de9fd..bbaccf11 100644 --- a/Code/OysterMath/OysterMath.cpp +++ b/Code/OysterMath/OysterMath.cpp @@ -81,20 +81,20 @@ namespace Oyster { namespace Math2D namespace Oyster { namespace Math3D { - Float4 AngularAxis( const Float3x3 &rotationMatrix ) - { - return ::LinearAlgebra3D::AngularAxis( rotationMatrix ); - } + //Float4 AngularAxis( const Float3x3 &rotationMatrix ) + //{ + // return ::LinearAlgebra3D::AngularAxis( rotationMatrix ); + //} - Float4 AngularAxis( const Float4x4 &rotationMatrix ) - { - return ::LinearAlgebra3D::AngularAxis( rotationMatrix ); - } + //Float4 AngularAxis( const Float4x4 &rotationMatrix ) + //{ + // return ::LinearAlgebra3D::AngularAxis( rotationMatrix ); + //} - Float4 ExtractAngularAxis( const Float4x4 &orientationMatrix ) - { - return ::LinearAlgebra3D::ExtractAngularAxis( orientationMatrix ); - } + //Float4 ExtractAngularAxis( const Float4x4 &orientationMatrix ) + //{ + // return ::LinearAlgebra3D::ExtractAngularAxis( orientationMatrix ); + //} Float4x4 & TranslationMatrix( const Float3 &position, Float4x4 &targetMem ) { diff --git a/Code/OysterMath/OysterMath.h b/Code/OysterMath/OysterMath.h index 3770bf02..5970face 100644 --- a/Code/OysterMath/OysterMath.h +++ b/Code/OysterMath/OysterMath.h @@ -57,59 +57,38 @@ namespace Oyster { namespace Math //! Oyster's native math library * @return nullvector if Lerp( start, end, t ) is nullvector. ********************************************************************/ using ::LinearAlgebra::Nlerp; + + /******************************************************************** + * Spherical Linear Interpolation on Quaternions + ********************************************************************/ + using ::LinearAlgebra::Slerp; } } inline ::Oyster::Math::Float2 & operator *= ( ::Oyster::Math::Float2 &left, const ::Oyster::Math::Float2 &right ) { - left.x *= right.x; - left.y *= right.y; - return left; + return left.PiecewiseMultiplicationAdd( right ); } inline ::Oyster::Math::Float2 operator * ( const ::Oyster::Math::Float2 &left, const ::Oyster::Math::Float2 &right ) -{ return ::Oyster::Math::Float2(left) *= right; } - -inline ::Oyster::Math::Float2 operator * ( const ::Oyster::Math::Float &left, const ::Oyster::Math::Float2 &right ) -{ return ::Oyster::Math::Float2(right) *= left; } +{ + return left.PiecewiseMultiplication( right ); +} inline ::Oyster::Math::Float3 & operator *= ( ::Oyster::Math::Float3 &left, const ::Oyster::Math::Float3 &right ) { - left.x *= right.x; - left.y *= right.y; - left.z *= right.z; - return left; + return left.PiecewiseMultiplicationAdd( right ); } inline ::Oyster::Math::Float3 operator * ( const ::Oyster::Math::Float3 &left, const ::Oyster::Math::Float3 &right ) -{ return ::Oyster::Math::Float3(left) *= right; } - -inline ::Oyster::Math::Float3 operator * ( const ::Oyster::Math::Float &left, const ::Oyster::Math::Float3 &right ) -{ return ::Oyster::Math::Float3(right) *= left; } +{ + return left.PiecewiseMultiplication( right ); +} inline ::Oyster::Math::Float4 & operator *= ( ::Oyster::Math::Float4 &left, const ::Oyster::Math::Float4 &right ) { - left.x *= right.x; - left.y *= right.y; - left.z *= right.z; - left.w *= right.w; - return left; + return left.PiecewiseMultiplicationAdd( right ); } -inline ::Oyster::Math::Float4 operator * ( const ::Oyster::Math::Float4 &left, const ::Oyster::Math::Float4 &right ) -{ return ::Oyster::Math::Float4(left) *= right; } - -inline ::Oyster::Math::Float4 operator * ( const ::Oyster::Math::Float &left, const ::Oyster::Math::Float4 &right ) -{ return ::Oyster::Math::Float4(right) *= left; } - -inline ::Oyster::Math::Float2x2 operator * ( const ::Oyster::Math::Float &left, const ::Oyster::Math::Float2x2 &right ) -{ return ::Oyster::Math::Float2x2(right) *= left; } - -inline ::Oyster::Math::Float3x3 operator * ( const ::Oyster::Math::Float &left, const ::Oyster::Math::Float3x3 &right ) -{ return ::Oyster::Math::Float3x3(right) *= left; } - -inline ::Oyster::Math::Float4x4 operator * ( const ::Oyster::Math::Float &left, const ::Oyster::Math::Float4x4 &right ) -{ return ::Oyster::Math::Float4x4(right) *= left; } - namespace Oyster { namespace Math2D //! Oyster's native math library specialized for 2D { using namespace ::Oyster::Math; // deliberate inheritance from ::Oyster::Math namespace @@ -162,13 +141,13 @@ namespace Oyster { namespace Math3D //! Oyster's native math library specialized using namespace ::Oyster::Math; // deliberate inheritance from ::Oyster::Math namespace //! Extracts the angularAxis from rotationMatrix - Float4 AngularAxis( const Float3x3 &rotationMatrix ); + //Float4 AngularAxis( const Float3x3 &rotationMatrix ); - //! Extracts the angularAxis from rotationMatrix - Float4 AngularAxis( const Float4x4 &rotationMatrix ); + ////! Extracts the angularAxis from rotationMatrix + //Float4 AngularAxis( const Float4x4 &rotationMatrix ); - //! Extracts the angularAxis from orientationMatrix - Float4 ExtractAngularAxis( const Float4x4 &orientationMatrix ); + ////! Extracts the angularAxis from orientationMatrix + //Float4 ExtractAngularAxis( const Float4x4 &orientationMatrix ); //! Sets and returns targetMem to a translationMatrix with position as translation. Float4x4 & TranslationMatrix( const Float3 &position, Float4x4 &targetMem = Float4x4() ); @@ -343,6 +322,8 @@ namespace Oyster { namespace Math3D //! Oyster's native math library specialized using ::LinearAlgebra3D::SnapAxisYToNormal_UsingNlerp; using ::LinearAlgebra3D::InterpolateAxisYToNormal_UsingNlerp; + using ::LinearAlgebra3D::InterpolateOrientation_UsingNonRigidNlerp; + using ::LinearAlgebra3D::InterpolateOrientation_UsingSlerp; } } #endif \ No newline at end of file diff --git a/Code/OysterMath/Vector.h b/Code/OysterMath/Vector.h index 901ea5e4..ea36cfc9 100644 --- a/Code/OysterMath/Vector.h +++ b/Code/OysterMath/Vector.h @@ -57,6 +57,12 @@ namespace LinearAlgebra ScalarType GetMagnitude( ) const; ScalarType Dot( const Vector2 &vector ) const; + //! @return (a.x * b.x, a.y * b.y) + Vector2 PiecewiseMultiplication( const Vector2 &vector ) const; + + //! @return a = (a.x * b.x, a.y * b.y) + Vector2 & PiecewiseMultiplicationAdd( const Vector2 &vector ); + Vector2 & Normalize( ); Vector2 GetNormalized( ) const; }; @@ -112,6 +118,12 @@ namespace LinearAlgebra ScalarType Dot( const Vector3 &vector ) const; Vector3 Cross( const Vector3 &vector ) const; + //! @return (a.x * b.x, a.y * b.y, a.z * b.z) + Vector3 PiecewiseMultiplication( const Vector3 &vector ) const; + + //! @return a = (a.x * b.x, a.y * b.y, a.z * b.z) + Vector3 & PiecewiseMultiplicationAdd( const Vector3 &vector ); + Vector3 & Normalize( ); Vector3 GetNormalized( ) const; }; @@ -169,14 +181,27 @@ namespace LinearAlgebra ScalarType GetMagnitude( ) const; ScalarType Dot( const Vector4 &vector ) const; + //! @return (a.x * b.x, a.y * b.y, a.z * b.z, a.w * b.w) + Vector4 PiecewiseMultiplication( const Vector4 &vector ) const; + + //! @return a = (a.x * b.x, a.y * b.y, a.z * b.z, a.w * b.w) + Vector4 & PiecewiseMultiplicationAdd( const Vector4 &vector ); + Vector4 & Normalize( ); Vector4 GetNormalized( ) const; }; +} + +template ::LinearAlgebra::Vector2 operator * ( const ScalarType &left, const ::LinearAlgebra::Vector2 &right ); +template ::LinearAlgebra::Vector3 operator * ( const ScalarType &left, const ::LinearAlgebra::Vector3 &right ); +template ::LinearAlgebra::Vector4 operator * ( const ScalarType &left, const ::LinearAlgebra::Vector4 &right ); /////////////////////////////////////////////////////////////////////////////////// // Body /////////////////////////////////////////////////////////////////////////////////// +namespace LinearAlgebra +{ // Vector2 /////////////////////////////////////// template const Vector2 Vector2::null = Vector2( 0, 0 ); @@ -184,22 +209,22 @@ namespace LinearAlgebra template const Vector2 Vector2::standard_unit_y = Vector2( 0, 1 ); template - Vector2::Vector2( ) : x(), y() {} + inline Vector2::Vector2( ) : x(), y() {} template - Vector2::Vector2( const Vector2 &vector ) + inline Vector2::Vector2( const Vector2 &vector ) { this->x = vector.x; this->y = vector.y; } template - Vector2::Vector2( const ScalarType &_element ) + inline Vector2::Vector2( const ScalarType &_element ) { this->x = this->y = _element; } template - Vector2::Vector2( const ScalarType _element[2] ) + inline Vector2::Vector2( const ScalarType _element[2] ) { this->x = _element[0]; this->y = _element[1]; } template - Vector2::Vector2( const ScalarType &_x, const ScalarType &_y ) + inline Vector2::Vector2( const ScalarType &_x, const ScalarType &_y ) { this->x = _x; this->y = _y; } template @@ -227,7 +252,7 @@ namespace LinearAlgebra { return this->element[i]; } template - Vector2 & Vector2::operator = ( const Vector2 &vector ) + inline Vector2 & Vector2::operator = ( const Vector2 &vector ) { this->element[0] = vector.element[0]; this->element[1] = vector.element[1]; @@ -235,7 +260,7 @@ namespace LinearAlgebra } template - Vector2 & Vector2::operator = ( const ScalarType _element[2] ) + inline Vector2 & Vector2::operator = ( const ScalarType _element[2] ) { this->element[0] = _element[0]; this->element[1] = _element[1]; @@ -243,7 +268,7 @@ namespace LinearAlgebra } template - Vector2 & Vector2::operator *= ( const ScalarType &scalar ) + inline Vector2 & Vector2::operator *= ( const ScalarType &scalar ) { this->element[0] *= scalar; this->element[1] *= scalar; @@ -251,7 +276,7 @@ namespace LinearAlgebra } template - Vector2 & Vector2::operator /= ( const ScalarType &scalar ) + inline Vector2 & Vector2::operator /= ( const ScalarType &scalar ) { this->element[0] /= scalar; this->element[1] /= scalar; @@ -259,7 +284,7 @@ namespace LinearAlgebra } template - Vector2 & Vector2::operator += ( const Vector2 &vector ) + inline Vector2 & Vector2::operator += ( const Vector2 &vector ) { this->element[0] += vector.element[0]; this->element[1] += vector.element[1]; @@ -267,7 +292,7 @@ namespace LinearAlgebra } template - Vector2 & Vector2::operator -= ( const Vector2 &vector ) + inline Vector2 & Vector2::operator -= ( const Vector2 &vector ) { this->element[0] -= vector.element[0]; this->element[1] -= vector.element[1]; @@ -295,7 +320,7 @@ namespace LinearAlgebra { return Vector2(-this->x, -this->y); } template - bool Vector2::operator == ( const Vector2 &vector ) const + inline bool Vector2::operator == ( const Vector2 &vector ) const { if( this->x != vector.x ) return false; if( this->y != vector.y ) return false; @@ -303,7 +328,7 @@ namespace LinearAlgebra } template - bool Vector2::operator != ( const Vector2 &vector ) const + inline bool Vector2::operator != ( const Vector2 &vector ) const { if( this->x != vector.x ) return true; if( this->y != vector.y ) return true; @@ -319,7 +344,7 @@ namespace LinearAlgebra { return (ScalarType) ::sqrt( this->Dot(*this) ); } template - ScalarType Vector2::Dot( const Vector2 &vector ) const + inline ScalarType Vector2::Dot( const Vector2 &vector ) const { ScalarType value = 0; value += this->element[0] * vector.element[0]; @@ -327,6 +352,20 @@ namespace LinearAlgebra return value; } + template + inline Vector2 Vector2::PiecewiseMultiplication( const Vector2 &vector ) const + { + return Vector2( this->x * vector.x, this->y * vector.y ); + } + + template + inline Vector2 & Vector2::PiecewiseMultiplicationAdd( const Vector2 &vector ) + { + this->x *= vector.x; + this->y *= vector.y; + return *this; + } + template inline Vector2 & Vector2::Normalize( ) { return (*this) /= this->GetLength(); } @@ -343,26 +382,26 @@ namespace LinearAlgebra template const Vector3 Vector3::standard_unit_z = Vector3( 0, 0, 1 ); template - Vector3::Vector3( ) : x(), y(), z() {} + inline Vector3::Vector3( ) : x(), y(), z() {} template - Vector3::Vector3( const Vector3 &vector ) + inline Vector3::Vector3( const Vector3 &vector ) { this->x = vector.x; this->y = vector.y; this->z = vector.z; } template - Vector3::Vector3( const Vector2 &vector, const ScalarType &_z ) + inline Vector3::Vector3( const Vector2 &vector, const ScalarType &_z ) { this->x = vector.x; this->y = vector.y; this->z = _z; } template - Vector3::Vector3( const ScalarType &_element ) + inline Vector3::Vector3( const ScalarType &_element ) { this->x = this->y = this->z = _element; } template - Vector3::Vector3( const ScalarType _element[3] ) + inline Vector3::Vector3( const ScalarType _element[3] ) { this->x = _element[0]; this->y = _element[1]; this->z = _element[2]; } template - Vector3::Vector3( const ScalarType &_x, const ScalarType &_y, const ScalarType &_z ) + inline Vector3::Vector3( const ScalarType &_x, const ScalarType &_y, const ScalarType &_z ) { this->x = _x; this->y = _y; this->z = _z; } template @@ -382,7 +421,7 @@ namespace LinearAlgebra { return this->element[i]; } template - Vector3 & Vector3::operator = ( const Vector3 &vector ) + inline Vector3 & Vector3::operator = ( const Vector3 &vector ) { this->element[0] = vector.element[0]; this->element[1] = vector.element[1]; @@ -391,7 +430,7 @@ namespace LinearAlgebra } template - Vector3 & Vector3::operator = ( const ScalarType element[3] ) + inline Vector3 & Vector3::operator = ( const ScalarType element[3] ) { this->element[0] = element[0]; this->element[1] = element[1]; @@ -400,7 +439,7 @@ namespace LinearAlgebra } template - Vector3 & Vector3::operator *= ( const ScalarType &scalar ) + inline Vector3 & Vector3::operator *= ( const ScalarType &scalar ) { this->element[0] *= scalar; this->element[1] *= scalar; @@ -409,7 +448,7 @@ namespace LinearAlgebra } template - Vector3 & Vector3::operator /= ( const ScalarType &scalar ) + inline Vector3 & Vector3::operator /= ( const ScalarType &scalar ) { this->element[0] /= scalar; this->element[1] /= scalar; @@ -418,7 +457,7 @@ namespace LinearAlgebra } template - Vector3 & Vector3::operator += ( const Vector3 &vector ) + inline Vector3 & Vector3::operator += ( const Vector3 &vector ) { this->element[0] += vector.element[0]; this->element[1] += vector.element[1]; @@ -427,7 +466,7 @@ namespace LinearAlgebra } template - Vector3 & Vector3::operator -= ( const Vector3 &vector ) + inline Vector3 & Vector3::operator -= ( const Vector3 &vector ) { this->element[0] -= vector.element[0]; this->element[1] -= vector.element[1]; @@ -456,7 +495,7 @@ namespace LinearAlgebra { return Vector3(-this->x, -this->y, -this->z); } template - bool Vector3::operator == ( const Vector3 &vector ) const + inline bool Vector3::operator == ( const Vector3 &vector ) const { if( this->x != vector.x ) return false; if( this->y != vector.y ) return false; @@ -465,7 +504,7 @@ namespace LinearAlgebra } template - bool Vector3::operator != ( const Vector3 &vector ) const + inline bool Vector3::operator != ( const Vector3 &vector ) const { if( this->x != vector.x ) return true; if( this->y != vector.y ) return true; @@ -482,7 +521,7 @@ namespace LinearAlgebra { return (ScalarType) ::sqrt( this->Dot(*this) ); } template - ScalarType Vector3::Dot( const Vector3 &vector ) const + inline ScalarType Vector3::Dot( const Vector3 &vector ) const { ScalarType value = 0; value += this->element[0] * vector.element[0]; @@ -492,13 +531,28 @@ namespace LinearAlgebra } template - Vector3 Vector3::Cross( const Vector3 &vector ) const + inline Vector3 Vector3::Cross( const Vector3 &vector ) const { return Vector3( (this->y*vector.z) - (this->z*vector.y), (this->z*vector.x) - (this->x*vector.z), (this->x*vector.y) - (this->y*vector.x) ); } + template + inline Vector3 Vector3::PiecewiseMultiplication( const Vector3 &vector ) const + { + return Vector3( this->x * vector.x, this->y * vector.y, this->z * vector.z ); + } + + template + inline Vector3 & Vector3::PiecewiseMultiplicationAdd( const Vector3 &vector ) + { + this->x *= vector.x; + this->y *= vector.y; + this->z *= vector.z; + return *this; + } + template inline Vector3 & Vector3::Normalize( ) { return (*this) /= this->GetLength(); } @@ -516,30 +570,30 @@ namespace LinearAlgebra template const Vector4 Vector4::standard_unit_w = Vector4( 0, 0, 0, 1 ); template - Vector4::Vector4( ) : x(), y(), z(), w() {} + inline Vector4::Vector4( ) : x(), y(), z(), w() {} template - Vector4::Vector4( const Vector4 &vector ) + inline Vector4::Vector4( const Vector4 &vector ) { this->x = vector.x; this->y = vector.y; this->z = vector.z; this->w = vector.w; } template - Vector4::Vector4( const Vector3 &vector, const ScalarType &_w ) + inline Vector4::Vector4( const Vector3 &vector, const ScalarType &_w ) { this->x = vector.x; this->y = vector.y; this->z = vector.z; this->w = _w; } template - Vector4::Vector4( const Vector2 &vector, const ScalarType &_z, const ScalarType &_w ) + inline Vector4::Vector4( const Vector2 &vector, const ScalarType &_z, const ScalarType &_w ) { this->x = vector.x; this->y = vector.y; this->z = _z; this->w = _w; } template - Vector4::Vector4( const ScalarType &_element ) + inline Vector4::Vector4( const ScalarType &_element ) { this->x = this->y = this->z = this->w = _element; } template - Vector4::Vector4( const ScalarType _element[4] ) + inline Vector4::Vector4( const ScalarType _element[4] ) { this->x = _element[0]; this->y = _element[1]; this->z = _element[2]; this->w = _element[3]; } template - Vector4::Vector4( const ScalarType &_x, const ScalarType &_y, const ScalarType &_z, const ScalarType &_w ) + inline Vector4::Vector4( const ScalarType &_x, const ScalarType &_y, const ScalarType &_z, const ScalarType &_w ) { this->x = _x; this->y = _y; this->z = _z; this->w = _w; } template @@ -559,7 +613,7 @@ namespace LinearAlgebra { return this->element[i]; } template - Vector4 & Vector4::operator = ( const Vector4 &vector ) + inline Vector4 & Vector4::operator = ( const Vector4 &vector ) { this->element[0] = vector.element[0]; this->element[1] = vector.element[1]; @@ -569,7 +623,7 @@ namespace LinearAlgebra } template - Vector4 & Vector4::operator = ( const ScalarType element[4] ) + inline Vector4 & Vector4::operator = ( const ScalarType element[4] ) { this->element[0] = element[0]; this->element[1] = element[1]; @@ -579,7 +633,7 @@ namespace LinearAlgebra } template - Vector4 & Vector4::operator *= ( const ScalarType &scalar ) + inline Vector4 & Vector4::operator *= ( const ScalarType &scalar ) { this->element[0] *= scalar; this->element[1] *= scalar; @@ -589,7 +643,7 @@ namespace LinearAlgebra } template - Vector4 & Vector4::operator /= ( const ScalarType &scalar ) + inline Vector4 & Vector4::operator /= ( const ScalarType &scalar ) { this->element[0] /= scalar; this->element[1] /= scalar; @@ -599,7 +653,7 @@ namespace LinearAlgebra } template - Vector4 & Vector4::operator += ( const Vector4 &vector ) + inline Vector4 & Vector4::operator += ( const Vector4 &vector ) { this->element[0] += vector.element[0]; this->element[1] += vector.element[1]; @@ -609,7 +663,7 @@ namespace LinearAlgebra } template - Vector4 & Vector4::operator -= ( const Vector4 &vector ) + inline Vector4 & Vector4::operator -= ( const Vector4 &vector ) { this->element[0] -= vector.element[0]; this->element[1] -= vector.element[1]; @@ -639,7 +693,7 @@ namespace LinearAlgebra { return Vector4(-this->x, -this->y, -this->z, -this->w); } template - bool Vector4::operator == ( const Vector4 &vector ) const + inline bool Vector4::operator == ( const Vector4 &vector ) const { if( this->x != vector.x ) return false; if( this->y != vector.y ) return false; @@ -649,7 +703,7 @@ namespace LinearAlgebra } template - bool Vector4::operator != ( const Vector4 &vector ) const + inline bool Vector4::operator != ( const Vector4 &vector ) const { if( this->x != vector.x ) return true; if( this->y != vector.y ) return true; @@ -667,7 +721,7 @@ namespace LinearAlgebra { return (ScalarType) ::sqrt( this->Dot(*this) ); } template - ScalarType Vector4::Dot( const Vector4 &vector ) const + inline ScalarType Vector4::Dot( const Vector4 &vector ) const { ScalarType value = 0; value += this->element[0] * vector.element[0]; @@ -677,6 +731,22 @@ namespace LinearAlgebra return value; } + template + inline Vector4 Vector4::PiecewiseMultiplication( const Vector4 &vector ) const + { + return Vector4( this->x * vector.x, this->y * vector.y, this->z * vector.z, this->w * vector.w ); + } + + template + inline Vector4 & Vector4::PiecewiseMultiplicationAdd( const Vector4 &vector ) + { + this->x *= vector.x; + this->y *= vector.y; + this->z *= vector.z; + this->w *= vector.w; + return *this; + } + template inline Vector4 & Vector4::Normalize( ) { return (*this) /= this->GetLength(); } @@ -686,4 +756,22 @@ namespace LinearAlgebra { return Vector4(*this).Normalize(); } } +template +inline ::LinearAlgebra::Vector2 operator * ( const ScalarType &left, const ::LinearAlgebra::Vector2 &right ) +{ + return right * left; +} + +template +inline ::LinearAlgebra::Vector3 operator * ( const ScalarType &left, const ::LinearAlgebra::Vector3 &right ) +{ + return right * left; +} + +template +inline ::LinearAlgebra::Vector4 operator * ( const ScalarType &left, const ::LinearAlgebra::Vector4 &right ) +{ + return right * left; +} + #endif \ No newline at end of file diff --git a/Code/OysterPhysics3D/Frustrum.cpp b/Code/OysterPhysics3D/Frustrum.cpp index 54e6a67f..ce2c5256 100644 --- a/Code/OysterPhysics3D/Frustrum.cpp +++ b/Code/OysterPhysics3D/Frustrum.cpp @@ -241,4 +241,9 @@ bool Frustrum::Contains( const ICollideable &target ) const //case Type_line: return false; // TODO: default: return false; } +} + +::Oyster::Math::Float3 Frustrum::ExtractForwad() +{ + return this->bottomPlane.normal.xyz; } \ No newline at end of file diff --git a/Code/OysterPhysics3D/Frustrum.h b/Code/OysterPhysics3D/Frustrum.h index ba5656c5..ae0f086c 100644 --- a/Code/OysterPhysics3D/Frustrum.h +++ b/Code/OysterPhysics3D/Frustrum.h @@ -41,6 +41,8 @@ namespace Oyster { namespace Collision3D bool Intersects( const ICollideable &target ) const; bool Intersects( const ICollideable &target, Oyster::Math::Float4 &worldPointOfContact ) const; bool Contains( const ICollideable &target ) const; + + ::Oyster::Math::Float3 ExtractForwad(); }; // INLINE IMPLEMENTATIONS /////////////////////////////////////// diff --git a/Code/OysterPhysics3D/Inertia.cpp b/Code/OysterPhysics3D/Inertia.cpp new file mode 100644 index 00000000..2e0e436b --- /dev/null +++ b/Code/OysterPhysics3D/Inertia.cpp @@ -0,0 +1,51 @@ +/******************************************************************** + * Created by Dan Andersson 2014 + ********************************************************************/ + +#include "Inertia.h" + +using namespace ::Oyster::Math3D; +using namespace ::Oyster::Physics3D; + +MomentOfInertia::MomentOfInertia() +{ + this->rotation = Quaternion::identity; + this->magnitude = Float3( 1.0f ); +} + +MomentOfInertia::MomentOfInertia( const Quaternion &r, const Float3 &m ) +{ + this->rotation = r; + this->magnitude = m; +} + +MomentOfInertia & MomentOfInertia::operator = ( const MomentOfInertia &i ) +{ + this->rotation = i.rotation; + this->magnitude = i.magnitude; + return *this; +} + +Float3 MomentOfInertia::CalculateAngularVelocity( const Quaternion &externR, const Float3 &h ) const +{ + return this->CalculateAngularVelocity( externR, h, Float3() ); +} + +Float3 & MomentOfInertia::CalculateAngularVelocity( const Quaternion &externR, const Float3 &h, Float3 &targetMem ) const +{ // w = (R * I_R) * I_M^-1 * (R * I_R)^-1 * h + Float4x4 rotation = RotationMatrix( externR ) * RotationMatrix( this->rotation ); + Float4 w = rotation.GetInverse() * Float4( h, 0.0f ); + return targetMem = rotation * w.PiecewiseMultiplicationAdd( Float4(1.0f / this->magnitude.x, 1.0f / this->magnitude.y, 1.0f / this->magnitude.z, 0.0f) ); +} + +Float3 MomentOfInertia::CalculateAngularMomentum( const Quaternion &externR, const Float3 &w ) const +{ + return this->CalculateAngularMomentum( externR, w, Float3() ); +} + +Float3 & MomentOfInertia::CalculateAngularMomentum( const Quaternion &externR, const Float3 &w, Float3 &targetMem ) const +{ // h = (R * I_R) * I_M * (R * I_R)^-1 * w + Float4x4 rotation = RotationMatrix( externR ) * RotationMatrix( this->rotation ); + Float4 h = rotation.GetInverse() * Float4( w, 0.0f ); + return targetMem = rotation * h.PiecewiseMultiplicationAdd( Float4(this->magnitude.x, this->magnitude.y, this->magnitude.z, 0.0f) ); +} \ No newline at end of file diff --git a/Code/OysterPhysics3D/Inertia.h b/Code/OysterPhysics3D/Inertia.h new file mode 100644 index 00000000..502074a6 --- /dev/null +++ b/Code/OysterPhysics3D/Inertia.h @@ -0,0 +1,119 @@ +/******************************************************************** + * Created by Dan Andersson & Robin Engman 2014 + ********************************************************************/ + +#ifndef OYSTER_PHYSICS_3D_INERTIA_H +#define OYSTER_PHYSICS_3D_INERTIA_H + +#include "OysterMath.h" + +namespace Oyster { namespace Physics3D +{ + struct MomentOfInertia + { + ::Oyster::Math::Quaternion rotation; + ::Oyster::Math::Float3 magnitude; + + MomentOfInertia(); + MomentOfInertia( const ::Oyster::Math::Quaternion &r, const ::Oyster::Math::Float3 &m ); + + MomentOfInertia & operator = ( const MomentOfInertia &i ); + + ::Oyster::Math::Float3 CalculateAngularVelocity( const ::Oyster::Math::Quaternion &externR, const ::Oyster::Math::Float3 &angularMomentum ) const; + ::Oyster::Math::Float3 & CalculateAngularVelocity( const ::Oyster::Math::Quaternion &externR, const ::Oyster::Math::Float3 &angularMomentum, ::Oyster::Math::Float3 &targetMem ) const; + + ::Oyster::Math::Float3 CalculateAngularMomentum( const ::Oyster::Math::Quaternion &externR, const ::Oyster::Math::Float3 &angularVelocity ) const; + ::Oyster::Math::Float3 & CalculateAngularMomentum( const ::Oyster::Math::Quaternion &externR, const ::Oyster::Math::Float3 &angularVelocity, ::Oyster::Math::Float3 &targetMem ) const; + + static ::Oyster::Math::Float CalculateSphere( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float radius ); + static ::Oyster::Math::Float CalculateHollowSphere( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float radius ); + static ::Oyster::Math::Float CalculateCuboidX( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float height, const ::Oyster::Math::Float depth ); + static ::Oyster::Math::Float CalculateCuboidY( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float width, const ::Oyster::Math::Float depth ); + static ::Oyster::Math::Float CalculateCuboidZ( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float width, const ::Oyster::Math::Float height ); + static ::Oyster::Math::Float CalculateRodCenter( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float length ); + static ::Oyster::Math::Float CalculateCylinderXY( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float height, const ::Oyster::Math::Float radius ); + static ::Oyster::Math::Float CalculateCylinderZ( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float radius ); + + static MomentOfInertia Sphere( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float radius ); + static MomentOfInertia HollowSphere( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float radius ); + static MomentOfInertia Cuboid( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float height, const ::Oyster::Math::Float width, const ::Oyster::Math::Float depth ); + static MomentOfInertia RodCenter( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float length ); + static MomentOfInertia Cylinder( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float height, const ::Oyster::Math::Float radius ); + }; + + inline ::Oyster::Math::Float MomentOfInertia::CalculateSphere( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float radius ) + { + return (2.0f / 5.0f) * mass * radius * radius; + } + + inline ::Oyster::Math::Float MomentOfInertia::CalculateHollowSphere( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float radius ) + { + return (2.0f / 3.0f) * mass * radius * radius; + } + + inline ::Oyster::Math::Float MomentOfInertia::CalculateCuboidX( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float height, const ::Oyster::Math::Float depth ) + { + return (1.0f / 12.0f) * mass * (height * height + depth * depth); + } + + inline ::Oyster::Math::Float MomentOfInertia::CalculateCuboidY( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float width, const ::Oyster::Math::Float depth ) + { + return (1.0f / 12.0f) * mass * (width * width + depth * depth); + } + + inline ::Oyster::Math::Float MomentOfInertia::CalculateCuboidZ( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float width, const ::Oyster::Math::Float height ) + { + return (1.0f / 12.0f) * mass * (height * height + width * width); + } + + inline ::Oyster::Math::Float MomentOfInertia::CalculateRodCenter( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float length ) + { + return (1.0f / 12.0f) * mass * length * length; + } + + inline ::Oyster::Math::Float MomentOfInertia::CalculateCylinderXY( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float height, const ::Oyster::Math::Float radius ) + { + return (1.0f / 12.0f) * mass * (3.0f * radius * radius + height * height); + } + + inline ::Oyster::Math::Float MomentOfInertia::CalculateCylinderZ( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float radius ) + { + return 0.5f * mass * radius * radius; + } + + inline MomentOfInertia MomentOfInertia::Sphere( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float radius ) + { + return MomentOfInertia( ::Oyster::Math::Quaternion::identity, + ::Oyster::Math::Float3(MomentOfInertia::CalculateSphere(mass, radius)) ); + } + + inline MomentOfInertia MomentOfInertia::HollowSphere( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float radius ) + { + return MomentOfInertia( ::Oyster::Math::Quaternion::identity, + ::Oyster::Math::Float3(MomentOfInertia::CalculateHollowSphere(mass, radius)) ); + } + + inline MomentOfInertia MomentOfInertia::Cuboid( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float height, const ::Oyster::Math::Float width, const ::Oyster::Math::Float depth ) + { + return MomentOfInertia( ::Oyster::Math::Quaternion::identity, + ::Oyster::Math::Float3(MomentOfInertia::CalculateCuboidX(mass, height, depth), + MomentOfInertia::CalculateCuboidY(mass, width, depth), + MomentOfInertia::CalculateCuboidZ(mass, height, width)) ); + } + + inline MomentOfInertia MomentOfInertia::RodCenter( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float length ) + { + return MomentOfInertia( ::Oyster::Math::Quaternion::identity, + ::Oyster::Math::Float3(MomentOfInertia::CalculateRodCenter(mass , length)) ); + } + + inline MomentOfInertia MomentOfInertia::Cylinder( const ::Oyster::Math::Float mass, const ::Oyster::Math::Float height, const ::Oyster::Math::Float radius ) + { + ::Oyster::Math::Float cylinderXY = MomentOfInertia::CalculateCylinderXY( mass , height, radius ); + return MomentOfInertia( ::Oyster::Math::Quaternion::identity, + ::Oyster::Math::Float3(cylinderXY, cylinderXY, + MomentOfInertia::CalculateCylinderZ(mass, radius)) ); + } +} } + +#endif \ No newline at end of file diff --git a/Code/OysterPhysics3D/OysterPhysics3D.vcxproj b/Code/OysterPhysics3D/OysterPhysics3D.vcxproj index 186ea535..dffeca4f 100644 --- a/Code/OysterPhysics3D/OysterPhysics3D.vcxproj +++ b/Code/OysterPhysics3D/OysterPhysics3D.vcxproj @@ -155,6 +155,7 @@ + @@ -174,6 +175,7 @@ + diff --git a/Code/OysterPhysics3D/OysterPhysics3D.vcxproj.filters b/Code/OysterPhysics3D/OysterPhysics3D.vcxproj.filters index 02919d28..da336ce5 100644 --- a/Code/OysterPhysics3D/OysterPhysics3D.vcxproj.filters +++ b/Code/OysterPhysics3D/OysterPhysics3D.vcxproj.filters @@ -78,6 +78,9 @@ Header Files\Physics + + Header Files\Physics + @@ -125,5 +128,8 @@ Source Files\Physics + + Source Files\Physics + \ No newline at end of file diff --git a/Code/OysterPhysics3D/RigidBody.cpp b/Code/OysterPhysics3D/RigidBody.cpp index 80c800a6..8e5f5f2b 100644 --- a/Code/OysterPhysics3D/RigidBody.cpp +++ b/Code/OysterPhysics3D/RigidBody.cpp @@ -23,7 +23,7 @@ RigidBody::RigidBody( ) this->frictionCoeff_Static = 0.5f; this->frictionCoeff_Kinetic = 1.0f; this->mass = 10; - this->momentOfInertiaTensor = Float4x4::identity; + this->momentOfInertiaTensor = MomentOfInertia(); this->rotation = Quaternion::identity; } @@ -51,17 +51,18 @@ void RigidBody::Update_LeapFrog( Float updateFrameLength ) // updating the linear // ds = dt * Formula::LinearVelocity( m, avg_G ) = dt * avg_G / m = (dt / m) * avg_G this->centerPos += ( updateFrameLength / this->mass ) * AverageWithDelta( this->momentum_Linear, this->impulse_Linear ); - + // updating the angular - Float4x4 rotationMatrix; ::Oyster::Math3D::RotationMatrix( this->rotation, rotationMatrix ); + //Float4x4 rotationMatrix; ::Oyster::Math3D::RotationMatrix( this->rotation, rotationMatrix ); // Important! The member data is all world data except the Inertia tensor. Thus a new InertiaTensor needs to be created to be compatible with the rest of the world data. - Float4x4 wMomentOfInertiaTensor = TransformMatrix( rotationMatrix, this->momentOfInertiaTensor ); // RI + //Float4x4 wMomentOfInertiaTensor = TransformMatrix( rotationMatrix, this->momentOfInertiaTensor ); // RI // dO = dt * Formula::AngularVelocity( (RI)^-1, avg_H ) = dt * (RI)^-1 * avg_H //! HACK: @todo Rotation temporary disabled //this->axis += Radian( Formula::AngularVelocity(wMomentOfInertiaTensor.GetInverse(), AverageWithDelta(this->momentum_Angular, this->impulse_Angular)) ); - //this->rotation = Rotation( this->axis ); + this->axis += this->momentOfInertiaTensor.CalculateAngularVelocity( this->rotation, AverageWithDelta(this->momentum_Angular, this->impulse_Angular) ); + this->rotation = Rotation( this->axis ); // update momentums and clear impulse_Linear and impulse_Angular this->momentum_Linear += this->impulse_Linear; @@ -71,31 +72,32 @@ void RigidBody::Update_LeapFrog( Float updateFrameLength ) this->impulse_Angular = Float4::null; } -void RigidBody::Predict_LeapFrog( Float4 &outDeltaPos, Float4 &outDeltaAxis, const Float4 &actingLinearImpulse, const Float4 &actingAngularImpulse, Float deltaTime ) +void RigidBody::Predict_LeapFrog( Float3 &outDeltaPos, Float3 &outDeltaAxis, const Float3 &actingLinearImpulse, const Float3 &actingAngularImpulse, Float deltaTime ) { // updating the linear // ds = dt * Formula::LinearVelocity( m, avg_G ) = dt * avg_G / m = (dt / m) * avg_G outDeltaPos = ( deltaTime / this->mass ) * AverageWithDelta( this->momentum_Linear, actingLinearImpulse ); // updating the angular - Float4x4 rotationMatrix; ::Oyster::Math3D::RotationMatrix( this->rotation, rotationMatrix ); - Float4x4 wMomentOfInertiaTensor = TransformMatrix( rotationMatrix, this->momentOfInertiaTensor ); // RI + //Float4x4 rotationMatrix; ::Oyster::Math3D::RotationMatrix( this->rotation, rotationMatrix ); + //Float4x4 wMomentOfInertiaTensor = TransformMatrix( rotationMatrix, this->momentOfInertiaTensor ); // RI // dO = dt * Formula::AngularVelocity( (RI)^-1, avg_H ) = dt * (RI)^-1 * avg_H - outDeltaAxis = Formula::AngularVelocity( wMomentOfInertiaTensor.GetInverse(), AverageWithDelta(this->momentum_Angular, actingAngularImpulse) ); + //outDeltaAxis = Formula::AngularVelocity( wMomentOfInertiaTensor.GetInverse(), AverageWithDelta(this->momentum_Angular, actingAngularImpulse) ); + outDeltaAxis = this->momentOfInertiaTensor.CalculateAngularVelocity( this->rotation, AverageWithDelta(this->momentum_Angular, this->impulse_Angular) ); } -void RigidBody::Move( const Float4 &deltaPos, const Float4 &deltaAxis ) +void RigidBody::Move( const Float3 &deltaPos, const Float3 &deltaAxis ) { this->centerPos += deltaPos; this->axis += deltaAxis; this->rotation = Rotation( this->axis ); } -void RigidBody::ApplyImpulse( const Float4 &worldJ, const Float4 &atWorldPos ) +void RigidBody::ApplyImpulse( const Float3 &worldJ, const Float3 &atWorldPos ) { // by Dan Andersson - Float4 worldOffset = atWorldPos - this->centerPos; - if( worldOffset != Float4::null ) + Float3 worldOffset = atWorldPos - this->centerPos; + if( worldOffset != Float3::null ) { this->impulse_Linear += VectorProjection( worldJ, atWorldPos ); this->impulse_Angular += Formula::ImpulseTorque( worldJ, atWorldPos ); @@ -106,7 +108,7 @@ void RigidBody::ApplyImpulse( const Float4 &worldJ, const Float4 &atWorldPos ) } } -const Float4x4 & RigidBody::GetMomentOfInertia() const +const MomentOfInertia & RigidBody::GetMomentOfInertia() const { // by Dan Andersson return this->momentOfInertiaTensor; } @@ -116,7 +118,7 @@ Float RigidBody::GetMass() const return this->mass; } -const Quaternion & RigidBody::GetRotation() const +const Quaternion & RigidBody::GetRotationQuaternion() const { // by Dan Andersson return this->rotation; } @@ -136,46 +138,38 @@ Float4x4 RigidBody::GetView() const return ViewMatrix( this->rotation, this->centerPos ); } -Float4 RigidBody::GetVelocity_Linear() const +Float3 RigidBody::GetVelocity_Linear() const { // by Dan Andersson return Formula::LinearVelocity( this->mass, this->momentum_Linear ); } -Float4 RigidBody::GetVelocity_Angular() const +Float3 RigidBody::GetVelocity_Angular() const { // by Dan Andersson - return Formula::AngularVelocity( this->momentOfInertiaTensor.GetInverse(), this->momentum_Angular ); + return this->momentOfInertiaTensor.CalculateAngularVelocity( this->rotation, this->momentum_Angular ); } -Float4 RigidBody::GetLinearMomentum( const Float4 &atWorldPos ) const +Float3 RigidBody::GetLinearMomentum( const Float3 &atWorldPos ) const { // by Dan Andersson return this->momentum_Linear + Formula::TangentialLinearMomentum( this->momentum_Angular, atWorldPos - this->centerPos ); } -void RigidBody::SetMomentOfInertia_KeepVelocity( const Float4x4 &localTensorI ) +void RigidBody::SetMomentOfInertia_KeepVelocity( const MomentOfInertia &localTensorI ) { // by Dan Andersson - if( localTensorI.GetDeterminant() != 0.0f ) - { // insanity check! MomentOfInertiaTensor must be invertable - Float4x4 rotationMatrix; RotationMatrix( this->rotation, rotationMatrix ); - - Float4 w = Formula::AngularVelocity( (rotationMatrix * this->momentOfInertiaTensor).GetInverse(), this->momentum_Angular ); - this->momentOfInertiaTensor = localTensorI; - this->momentum_Angular = Formula::AngularMomentum( rotationMatrix * localTensorI, w ); - } + Float3 w = this->momentOfInertiaTensor.CalculateAngularVelocity( this->rotation, this->momentum_Angular ); + this->momentOfInertiaTensor = localTensorI; + this->momentum_Angular = this->momentOfInertiaTensor.CalculateAngularVelocity( this->rotation, w ); } -void RigidBody::SetMomentOfInertia_KeepMomentum( const Float4x4 &localTensorI ) +void RigidBody::SetMomentOfInertia_KeepMomentum( const MomentOfInertia &localTensorI ) { // by Dan Andersson - if( localTensorI.GetDeterminant() != 0.0f ) - { // insanity check! MomentOfInertiaTensor must be invertable - this->momentOfInertiaTensor = localTensorI; - } + this->momentOfInertiaTensor = localTensorI; } void RigidBody::SetMass_KeepVelocity( const Float &m ) { // by Dan Andersson if( m != 0.0f ) { // insanity check! Mass must be invertable - Float4 v = Formula::LinearVelocity( this->mass, this->momentum_Linear ); + Float3 v = Formula::LinearVelocity( this->mass, this->momentum_Linear ); this->mass = m; this->momentum_Linear = Formula::LinearMomentum( this->mass, v ); } @@ -189,46 +183,46 @@ void RigidBody::SetMass_KeepMomentum( const Float &m ) } } -void RigidBody::SetOrientation( const Float4x4 &o ) -{ // by Dan Andersson - this->axis = ExtractAngularAxis( o ); - this->rotation = Rotation( this->axis ); - this->centerPos = o.v[3].xyz; -} +//void RigidBody::SetOrientation( const Float4x4 &o ) +//{ // by Dan Andersson +// this->axis = ExtractAngularAxis( o ); +// this->rotation = Rotation( this->axis ); +// this->centerPos = o.v[3].xyz; +//} +// +//void RigidBody::SetRotation( const Float4x4 &r ) +//{ // by Dan Andersson +// this->axis = ExtractAngularAxis( r ); +// this->rotation = Rotation( this->axis ); +//} -void RigidBody::SetRotation( const Float4x4 &r ) +void RigidBody::SetMomentum_Linear( const Float3 &worldG, const Float3 &atWorldPos ) { // by Dan Andersson - this->axis = ExtractAngularAxis( r ); - this->rotation = Rotation( this->axis ); -} - -void RigidBody::SetMomentum_Linear( const Float4 &worldG, const Float4 &atWorldPos ) -{ // by Dan Andersson - Float4 worldOffset = atWorldPos - this->centerPos; + Float3 worldOffset = atWorldPos - this->centerPos; this->momentum_Linear = VectorProjection( worldG, worldOffset ); this->momentum_Angular = Formula::AngularMomentum( worldG, worldOffset ); } -void RigidBody::SetVelocity_Linear( const Float4 &worldV ) +void RigidBody::SetVelocity_Linear( const Float3 &worldV ) { // by Dan Andersson this->momentum_Linear = Formula::LinearMomentum( this->mass, worldV ); } -void RigidBody::SetVelocity_Linear( const Float4 &worldV, const Float4 &atWorldPos ) +void RigidBody::SetVelocity_Linear( const Float3 &worldV, const Float3 &atWorldPos ) { // by Dan Andersson - Float4 worldOffset = atWorldPos - this->centerPos; - this->momentum_Linear = Formula::LinearMomentum( this->mass, VectorProjection(worldV, worldOffset) ); - this->momentum_Angular = Formula::AngularMomentum( RotationMatrix(this->rotation) * this->momentOfInertiaTensor, Formula::AngularVelocity(worldV, worldOffset) ); + Float3 worldOffset = atWorldPos - this->centerPos; + this->momentum_Linear = Formula::LinearMomentum( this->mass, VectorProjection(worldV, worldOffset) ); + this->momentum_Angular = this->momentOfInertiaTensor.CalculateAngularMomentum( this->rotation, Formula::AngularVelocity(worldV, worldOffset) ); } -void RigidBody::SetVelocity_Angular( const Float4 &worldW ) +void RigidBody::SetVelocity_Angular( const Float3 &worldW ) { // by Dan Andersson - this->momentum_Angular = Formula::AngularMomentum( this->momentOfInertiaTensor, worldW ); + this->momentum_Angular = this->momentOfInertiaTensor.CalculateAngularMomentum( this->rotation, worldW ); } -void RigidBody::SetImpulse_Linear( const Float4 &worldJ, const Float4 &atWorldPos ) +void RigidBody::SetImpulse_Linear( const Float3 &worldJ, const Float3 &atWorldPos ) { // by Dan Andersson - Float4 worldOffset = atWorldPos - this->centerPos; + Float3 worldOffset = atWorldPos - this->centerPos; this->impulse_Linear = VectorProjection( worldJ, worldOffset ); this->impulse_Angular = Formula::ImpulseTorque( worldJ, worldOffset ); } \ No newline at end of file diff --git a/Code/OysterPhysics3D/RigidBody.h b/Code/OysterPhysics3D/RigidBody.h index 51c5d2d8..c666662a 100644 --- a/Code/OysterPhysics3D/RigidBody.h +++ b/Code/OysterPhysics3D/RigidBody.h @@ -8,13 +8,14 @@ #include "OysterMath.h" #include "OysterCollision3D.h" #include "OysterPhysics3D.h" +#include "Inertia.h" namespace Oyster { namespace Physics3D { struct RigidBody { //! A struct of a simple rigid body. public: - ::Oyster::Math::Float4 centerPos, //!< Location of the body's center in the world. + ::Oyster::Math::Float3 centerPos, //!< Location of the body's center in the world. axis, //!< Euler rotationAxis of the body. boundingReach, //!< momentum_Linear, //!< The linear momentum G (kg*m/s). @@ -31,54 +32,55 @@ namespace Oyster { namespace Physics3D RigidBody & operator = ( const RigidBody &body ); void Update_LeapFrog( ::Oyster::Math::Float updateFrameLength ); - void Predict_LeapFrog( ::Oyster::Math::Float4 &outDeltaPos, ::Oyster::Math::Float4 &outDeltaAxis, const ::Oyster::Math::Float4 &actingLinearImpulse, const ::Oyster::Math::Float4 &actingAngularImpulse, ::Oyster::Math::Float deltaTime ); + void Predict_LeapFrog( ::Oyster::Math::Float3 &outDeltaPos, ::Oyster::Math::Float3 &outDeltaAxis, const ::Oyster::Math::Float3 &actingLinearImpulse, const ::Oyster::Math::Float3 &actingAngularImpulse, ::Oyster::Math::Float deltaTime ); - void Move( const ::Oyster::Math::Float4 &deltaPos, const ::Oyster::Math::Float4 &deltaAxis ); - void ApplyImpulse( const ::Oyster::Math::Float4 &worldJ, const ::Oyster::Math::Float4 &atWorldPos ); - void ApplyImpulse_Linear( const ::Oyster::Math::Float4 &worldJ ); - void ApplyImpulse_Angular( const ::Oyster::Math::Float4 &worldJ ); - void ApplyForce( const ::Oyster::Math::Float4 &worldF, ::Oyster::Math::Float updateFrameLength ); - void ApplyForce( const ::Oyster::Math::Float4 &worldF, ::Oyster::Math::Float updateFrameLength, const ::Oyster::Math::Float4 &atWorldPos ); + void Move( const ::Oyster::Math::Float3 &deltaPos, const ::Oyster::Math::Float3 &deltaAxis ); + void ApplyImpulse( const ::Oyster::Math::Float3 &worldJ, const ::Oyster::Math::Float3 &atWorldPos ); + void ApplyImpulse_Linear( const ::Oyster::Math::Float3 &worldJ ); + void ApplyImpulse_Angular( const ::Oyster::Math::Float3 &worldJ ); + void ApplyForce( const ::Oyster::Math::Float3 &worldF, ::Oyster::Math::Float updateFrameLength ); + void ApplyForce( const ::Oyster::Math::Float3 &worldF, ::Oyster::Math::Float updateFrameLength, const ::Oyster::Math::Float3 &atWorldPos ); // GET METHODS //////////////////////////////// - const ::Oyster::Math::Float4x4 & GetMomentOfInertia() const; - ::Oyster::Math::Float GetMass() const; - const ::Oyster::Math::Quaternion & GetRotation() const; - ::Oyster::Math::Float4x4 GetRotationMatrix() const; - ::Oyster::Math::Float4x4 GetOrientation() const; - ::Oyster::Math::Float4x4 GetView() const; - ::Oyster::Math::Float4x4 GetToWorldMatrix() const; - ::Oyster::Math::Float4x4 GetToLocalMatrix() const; - ::Oyster::Math::Float4 GetSize() const; - ::Oyster::Math::Float4 GetVelocity_Linear() const; - ::Oyster::Math::Float4 GetVelocity_Angular() const; - ::Oyster::Math::Float4 GetLinearMomentum( const ::Oyster::Math::Float4 &atWorldPos ) const; + const ::Oyster::Physics3D::MomentOfInertia & GetMomentOfInertia() const; + ::Oyster::Math::Float GetMass() const; + const ::Oyster::Math::Quaternion & GetRotationQuaternion() const; + ::Oyster::Math::Float4x4 GetRotationMatrix() const; + ::Oyster::Math::Float4x4 GetOrientation() const; + ::Oyster::Math::Float4x4 GetView() const; + ::Oyster::Math::Float4x4 GetToWorldMatrix() const; + ::Oyster::Math::Float4x4 GetToLocalMatrix() const; + ::Oyster::Math::Float3 GetSize() const; + ::Oyster::Math::Float3 GetVelocity_Linear() const; + ::Oyster::Math::Float3 GetVelocity_Angular() const; + ::Oyster::Math::Float3 GetLinearMomentum( const ::Oyster::Math::Float3 &atWorldPos ) const; // SET METHODS //////////////////////////////// - void SetMomentOfInertia_KeepVelocity( const ::Oyster::Math::Float4x4 &localTensorI ); - void SetMomentOfInertia_KeepMomentum( const ::Oyster::Math::Float4x4 &localTensorI ); + void SetMomentOfInertia_KeepVelocity( const ::Oyster::Physics3D::MomentOfInertia &localTensorI ); + void SetMomentOfInertia_KeepMomentum( const ::Oyster::Physics3D::MomentOfInertia &localTensorI ); void SetMass_KeepVelocity( const ::Oyster::Math::Float &m ); void SetMass_KeepMomentum( const ::Oyster::Math::Float &m ); - void SetOrientation( const ::Oyster::Math::Float4x4 &o ); - void SetRotation( const ::Oyster::Math::Float4x4 &r ); - void SetSize( const ::Oyster::Math::Float4 &widthHeight ); + //void SetOrientation( const ::Oyster::Math::Float4x4 &o ); + //void SetRotation( const ::Oyster::Math::Float4x4 &r ); + void SetSize( const ::Oyster::Math::Float3 &widthHeight ); - void SetMomentum_Linear( const ::Oyster::Math::Float4 &worldG, const ::Oyster::Math::Float4 &atWorldPos ); + void SetMomentum_Linear( const ::Oyster::Math::Float3 &worldG, const ::Oyster::Math::Float3 &atWorldPos ); - void SetVelocity_Linear( const ::Oyster::Math::Float4 &worldV ); - void SetVelocity_Linear( const ::Oyster::Math::Float4 &worldV, const ::Oyster::Math::Float4 &atWorldPos ); - void SetVelocity_Angular( const ::Oyster::Math::Float4 &worldW ); + void SetVelocity_Linear( const ::Oyster::Math::Float3 &worldV ); + void SetVelocity_Linear( const ::Oyster::Math::Float3 &worldV, const ::Oyster::Math::Float3 &atWorldPos ); + void SetVelocity_Angular( const ::Oyster::Math::Float3 &worldW ); - void SetImpulse_Linear( const ::Oyster::Math::Float4 &worldJ, const ::Oyster::Math::Float4 &atWorldPos ); - void SetForce( const ::Oyster::Math::Float4 &worldF, ::Oyster::Math::Float updateFrameLength ); - void SetForce( const ::Oyster::Math::Float4 &worldF, ::Oyster::Math::Float updateFrameLength, const ::Oyster::Math::Float4 &atWorldPos ); + void SetImpulse_Linear( const ::Oyster::Math::Float3 &worldJ, const ::Oyster::Math::Float3 &atWorldPos ); + //void SetForce( const ::Oyster::Math::Float3 &worldF, ::Oyster::Math::Float updateFrameLength ); + //void SetForce( const ::Oyster::Math::Float3 &worldF, ::Oyster::Math::Float updateFrameLength, const ::Oyster::Math::Float3 &atWorldPos ); private: ::Oyster::Math::Float mass; //!< m (kg) - ::Oyster::Math::Float4x4 momentOfInertiaTensor; //!< I (Nm*s) Tensor matrix ( only need to be 3x3 matrix, but is 4x4 for future hardware acceleration ) (localValue) + //::Oyster::Math::Float4x4 momentOfInertiaTensor; //!< I (Nm*s) Tensor matrix ( only need to be 3x3 matrix, but is 4x4 for future hardware acceleration ) (localValue) + ::Oyster::Physics3D::MomentOfInertia momentOfInertiaTensor; ::Oyster::Math::Quaternion rotation; //!< RotationAxis of the body. }; } } diff --git a/Code/OysterPhysics3D/RigidBody_Inline.h b/Code/OysterPhysics3D/RigidBody_Inline.h index 980442dd..795ec5e4 100644 --- a/Code/OysterPhysics3D/RigidBody_Inline.h +++ b/Code/OysterPhysics3D/RigidBody_Inline.h @@ -10,22 +10,22 @@ namespace Oyster { namespace Physics3D { - inline void RigidBody::ApplyImpulse_Linear( const ::Oyster::Math::Float4 &worldJ ) + inline void RigidBody::ApplyImpulse_Linear( const ::Oyster::Math::Float3 &worldJ ) { this->impulse_Linear += worldJ; } - inline void RigidBody::ApplyImpulse_Angular( const ::Oyster::Math::Float4 &worldJ ) + inline void RigidBody::ApplyImpulse_Angular( const ::Oyster::Math::Float3 &worldJ ) { this->impulse_Angular += worldJ; } - inline void RigidBody::ApplyForce( const ::Oyster::Math::Float4 &worldF, ::Oyster::Math::Float updateFrameLength ) + inline void RigidBody::ApplyForce( const ::Oyster::Math::Float3 &worldF, ::Oyster::Math::Float updateFrameLength ) { this->impulse_Linear += worldF * updateFrameLength; } - inline void RigidBody::ApplyForce( const ::Oyster::Math::Float4 &worldF, ::Oyster::Math::Float updateFrameLength, const ::Oyster::Math::Float4 &atWorldPos ) + inline void RigidBody::ApplyForce( const ::Oyster::Math::Float3 &worldF, ::Oyster::Math::Float updateFrameLength, const ::Oyster::Math::Float3 &atWorldPos ) { this->ApplyImpulse( worldF * updateFrameLength, atWorldPos ); } @@ -40,26 +40,25 @@ namespace Oyster { namespace Physics3D return this->GetView(); } - inline ::Oyster::Math::Float4 RigidBody::GetSize() const + inline ::Oyster::Math::Float3 RigidBody::GetSize() const { return 2.0f * this->boundingReach; } - inline void RigidBody::SetSize( const ::Oyster::Math::Float4 &widthHeight ) + inline void RigidBody::SetSize( const ::Oyster::Math::Float3 &widthHeight ) { this->boundingReach = ::Utility::Value::Abs( 0.5f * widthHeight ); } - inline void RigidBody::SetForce( const ::Oyster::Math::Float4 &worldF, ::Oyster::Math::Float updateFrameLength ) - { - this->impulse_Linear = worldF * updateFrameLength; - } - - inline void RigidBody::SetForce( const ::Oyster::Math::Float4 &worldF, ::Oyster::Math::Float updateFrameLength, const ::Oyster::Math::Float4 &atWorldPos ) - { - this->SetImpulse_Linear( worldF * updateFrameLength, atWorldPos ); - } + //inline void RigidBody::SetForce( const ::Oyster::Math::Float3 &worldF, ::Oyster::Math::Float updateFrameLength ) + //{ + // this->impulse_Linear = worldF * updateFrameLength; + //} + //inline void RigidBody::SetForce( const ::Oyster::Math::Float3 &worldF, ::Oyster::Math::Float updateFrameLength, const ::Oyster::Math::Float3 &atWorldPos ) + //{ + // this->SetImpulse_Linear( worldF * updateFrameLength, atWorldPos ); + //} } } #endif \ No newline at end of file