NoEdge/Danbias
NoEdge
/
Danbias
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

GL - merge with physics

This commit is contained in:
lindaandersson 2014-01-28 15:43:32 +01:00
parent a56fcbcb9b 42addb9c06
commit 9ccfebd9fd
28 changed files with 1753 additions and 374 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
Code/Dokumentation/Other/Timestep_Impulse_Fix.odt Normal file
View File

Binary file not shown.

BIN
Code/Dokumentation/Other/Timestep_Impulse_Fix.pdf Normal file
View File

Binary file not shown.

BIN
Code/Dokumentation/Other/angular momentum to angular velocity.odt Normal file
View File

Binary file not shown.

BIN
Code/Dokumentation/Other/angular_momentum_to_angular_velocity.pdf Normal file
View File

Binary file not shown.

1059
Code/Dokumentation/Physics_Sprint3.uxf Normal file
View File

File diff suppressed because it is too large Load Diff

4
Code/GamePhysics/Implementation/Octree.cpp
View File

@ -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; i<this->leafData.size(); i++)
{
if(collideable.Intersects(this->leafData[i].container))
{
hitAction( this->GetCustomBody(i) );
hitAction( this->GetCustomBody(i), args );
}
}
}

4
Code/GamePhysics/Implementation/Octree.h
View File

@ -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<ICustomBody*>& Sample(ICustomBody* customBodyRef, std::vector<ICustomBody*>& updateList);
std::vector<ICustomBody*>& Sample(const Oyster::Collision3D::ICollideable& collideable, std::vector<ICustomBody*>& 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);

27
Code/GamePhysics/Implementation/PhysicsAPI_Impl.cpp
View File

@ -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 )

2
Code/GamePhysics/Implementation/PhysicsAPI_Impl.h
View File

@ -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 );

12
Code/GamePhysics/Implementation/SimpleRigidBody.cpp
View File

@ -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 )

14
Code/GamePhysics/Implementation/SphericalRigidBody.cpp
View File

@ -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 )

6
Code/GamePhysics/PhysicsAPI.h
View File

@ -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.

132
Code/GamePhysics/PhysicsStructs-Impl.h
View File

@ -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 &centerPos, 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 &centerPos, 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 &centerPos )
inline void CustomBodyState::SetCenterPosition( const ::Oyster::Math::Float3 &centerPos )
{
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;

104
Code/GamePhysics/PhysicsStructs.h
View File

@ -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 &centerPos = ::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 &centerPos = ::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 &centerPos );
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 &centerPos );
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;
};

84
Code/OysterMath/LinearMath.h
View File

@ -35,7 +35,7 @@ namespace std
// x2
template<typename ScalarType>
::LinearAlgebra::Matrix2x2<ScalarType> operator * ( const ::LinearAlgebra::Matrix2x2<ScalarType> &left, const ::LinearAlgebra::Matrix2x2<ScalarType> &right )
inline ::LinearAlgebra::Matrix2x2<ScalarType> operator * ( const ::LinearAlgebra::Matrix2x2<ScalarType> &left, const ::LinearAlgebra::Matrix2x2<ScalarType> &right )
{
return ::LinearAlgebra::Matrix2x2<ScalarType>( (left.m11 * right.m11) + (left.m12 * right.m21),
(left.m11 * right.m12) + (left.m12 * right.m22),
@ -44,14 +44,14 @@ template<typename ScalarType>
}
template<typename ScalarType>
::LinearAlgebra::Vector2<ScalarType> operator * ( const ::LinearAlgebra::Matrix2x2<ScalarType> &matrix, const ::LinearAlgebra::Vector2<ScalarType> &vector )
inline ::LinearAlgebra::Vector2<ScalarType> operator * ( const ::LinearAlgebra::Matrix2x2<ScalarType> &matrix, const ::LinearAlgebra::Vector2<ScalarType> &vector )
{
return ::LinearAlgebra::Vector2<ScalarType>( (matrix.m11 * vector.x) + (matrix.m12 * vector.y),
(matrix.m21 * vector.x) + (matrix.m22 * vector.y) );
}
template<typename ScalarType>
::LinearAlgebra::Vector2<ScalarType> operator * ( const ::LinearAlgebra::Vector2<ScalarType> &vector, const ::LinearAlgebra::Matrix2x2<ScalarType> &left )
inline ::LinearAlgebra::Vector2<ScalarType> operator * ( const ::LinearAlgebra::Vector2<ScalarType> &vector, const ::LinearAlgebra::Matrix2x2<ScalarType> &left )
{
return ::LinearAlgebra::Vector2<ScalarType>( (vector.x * matrix.m11) + (vector.y * matrix.m21),
(vector.x * matrix.m12) + (vector.y * matrix.m22) );
@ -60,7 +60,7 @@ template<typename ScalarType>
// x3
template<typename ScalarType>
::LinearAlgebra::Matrix3x3<ScalarType> operator * ( const ::LinearAlgebra::Matrix3x3<ScalarType> &left, const ::LinearAlgebra::Matrix3x3<ScalarType> &right )
inline ::LinearAlgebra::Matrix3x3<ScalarType> operator * ( const ::LinearAlgebra::Matrix3x3<ScalarType> &left, const ::LinearAlgebra::Matrix3x3<ScalarType> &right )
{
return ::LinearAlgebra::Matrix3x3<ScalarType>( (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<typename ScalarType>
}
template<typename ScalarType>
::LinearAlgebra::Vector3<ScalarType> operator * ( const ::LinearAlgebra::Matrix3x3<ScalarType> &matrix, const ::LinearAlgebra::Vector3<ScalarType> &vector )
inline ::LinearAlgebra::Vector3<ScalarType> operator * ( const ::LinearAlgebra::Matrix3x3<ScalarType> &matrix, const ::LinearAlgebra::Vector3<ScalarType> &vector )
{
return ::LinearAlgebra::Vector3<ScalarType>( (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<typename ScalarType>
}
template<typename ScalarType>
::LinearAlgebra::Vector3<ScalarType> operator * ( const ::LinearAlgebra::Vector3<ScalarType> &vector, const ::LinearAlgebra::Matrix3x3<ScalarType> &left )
inline ::LinearAlgebra::Vector3<ScalarType> operator * ( const ::LinearAlgebra::Vector3<ScalarType> &vector, const ::LinearAlgebra::Matrix3x3<ScalarType> &left )
{
return ::LinearAlgebra::Vector3<ScalarType>( (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<typename ScalarType>
// x4
template<typename ScalarType>
::LinearAlgebra::Matrix4x4<ScalarType> operator * ( const ::LinearAlgebra::Matrix4x4<ScalarType> &left, const ::LinearAlgebra::Matrix4x4<ScalarType> &right )
inline ::LinearAlgebra::Matrix4x4<ScalarType> operator * ( const ::LinearAlgebra::Matrix4x4<ScalarType> &left, const ::LinearAlgebra::Matrix4x4<ScalarType> &right )
{
return ::LinearAlgebra::Matrix4x4<ScalarType>( (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<typename ScalarType>
}
template<typename ScalarType>
::LinearAlgebra::Vector4<ScalarType> operator * ( const ::LinearAlgebra::Matrix4x4<ScalarType> &matrix, const ::LinearAlgebra::Vector4<ScalarType> &vector )
inline ::LinearAlgebra::Vector4<ScalarType> operator * ( const ::LinearAlgebra::Matrix4x4<ScalarType> &matrix, const ::LinearAlgebra::Vector4<ScalarType> &vector )
{
return ::LinearAlgebra::Vector4<ScalarType>( (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<typename ScalarType>
}
template<typename ScalarType>
::LinearAlgebra::Vector4<ScalarType> operator * ( const ::LinearAlgebra::Vector4<ScalarType> &vector, const ::LinearAlgebra::Matrix4x4<ScalarType> &matrix )
inline ::LinearAlgebra::Vector4<ScalarType> operator * ( const ::LinearAlgebra::Vector4<ScalarType> &vector, const ::LinearAlgebra::Matrix4x4<ScalarType> &matrix )
{
return ::LinearAlgebra::Vector4<ScalarType>( (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<typename ScalarType>
inline Quaternion<ScalarType> Slerp( const Quaternion<ScalarType> &start, const Quaternion<ScalarType> &end, const ScalarType &t )
{
ScalarType angle = (ScalarType)::std::acos( Vector4<ScalarType>(start.imaginary, start.real).Dot(Vector4<ScalarType>(end.imaginary, end.real)) );
Quaternion<ScalarType> 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<typename ScalarType>
inline ::LinearAlgebra::Vector4<ScalarType> AngularAxis( const ::LinearAlgebra::Matrix3x3<ScalarType> &rotationMatrix )
{
return ::std::asin( ::LinearAlgebra::Vector4<ScalarType>(rotationMatrix.v[1].z, rotationMatrix.v[2].x, rotationMatrix.v[0].y, 1) );
}
// All Matrix to AngularAxis conversions here is incorrect
//template<typename ScalarType>
//inline ::LinearAlgebra::Vector4<ScalarType> AngularAxis( const ::LinearAlgebra::Matrix3x3<ScalarType> &rotationMatrix )
//{
// return ::std::asin( ::LinearAlgebra::Vector4<ScalarType>(rotationMatrix.v[1].z, rotationMatrix.v[2].x, rotationMatrix.v[0].y, 1) );
//}
template<typename ScalarType>
inline ::LinearAlgebra::Vector4<ScalarType> AngularAxis( const ::LinearAlgebra::Matrix4x4<ScalarType> &rotationMatrix )
{
return ::std::asin( ::LinearAlgebra::Vector4<ScalarType>(rotationMatrix.v[1].z, rotationMatrix.v[2].x, rotationMatrix.v[0].y, 1) );
}
//template<typename ScalarType>
//inline ::LinearAlgebra::Vector4<ScalarType> AngularAxis( const ::LinearAlgebra::Matrix4x4<ScalarType> &rotationMatrix )
//{
// return ::std::asin( ::LinearAlgebra::Vector4<ScalarType>(rotationMatrix.v[1].z, rotationMatrix.v[2].x, rotationMatrix.v[0].y, 1) );
//}
template<typename ScalarType>
inline ::LinearAlgebra::Vector4<ScalarType> ExtractAngularAxis( const ::LinearAlgebra::Matrix4x4<ScalarType> &orientationMatrix )
{
return ::std::asin( ::LinearAlgebra::Vector4<ScalarType>(orientationMatrix.v[1].z, orientationMatrix.v[2].x, orientationMatrix.v[0].y, 0) );
}
//template<typename ScalarType>
//inline ::LinearAlgebra::Vector4<ScalarType> ExtractAngularAxis( const ::LinearAlgebra::Matrix4x4<ScalarType> &orientationMatrix )
//{
// return ::std::asin( ::LinearAlgebra::Vector4<ScalarType>(orientationMatrix.v[1].z, orientationMatrix.v[2].x, orientationMatrix.v[0].y, 0) );
//}
template<typename ScalarType>
inline ::LinearAlgebra::Matrix4x4<ScalarType> & TranslationMatrix( const ::LinearAlgebra::Vector3<ScalarType> &position, ::LinearAlgebra::Matrix4x4<ScalarType> &targetMem = ::LinearAlgebra::Matrix4x4<ScalarType>() )
@ -756,6 +770,28 @@ namespace LinearAlgebra3D
return rotation; // return no change
return SnapAxisYToNormal_UsingNlerp( rotation, interpolated );
}
template<typename ScalarType>
::LinearAlgebra::Matrix4x4<ScalarType> & InterpolateOrientation_UsingNonRigidNlerp( const ::LinearAlgebra::Matrix4x4<ScalarType> &start, const ::LinearAlgebra::Matrix4x4<ScalarType> &end, ScalarType t, ::LinearAlgebra::Matrix4x4<ScalarType> &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<typename ScalarType>
::LinearAlgebra::Matrix4x4<ScalarType> & InterpolateOrientation_UsingNonRigidNlerp( const ::LinearAlgebra::Quaternion<ScalarType> &startR, const ::LinearAlgebra::Vector3<ScalarType> &startT, const ::LinearAlgebra::Quaternion<ScalarType> &endR, const ::LinearAlgebra::Vector3<ScalarType> &endT, ScalarType t, ::LinearAlgebra::Matrix4x4<ScalarType> &targetMem )
{
return InterpolateOrientation_UsingNonRigidNlerp( OrientationMatrix(startR, startT), OrientationMatrix(endR, endT), t, targetMem );
}
template<typename ScalarType>
::LinearAlgebra::Matrix4x4<ScalarType> & InterpolateOrientation_UsingSlerp( const ::LinearAlgebra::Quaternion<ScalarType> &startR, const ::LinearAlgebra::Vector3<ScalarType> &startT, const ::LinearAlgebra::Quaternion<ScalarType> &endR, const ::LinearAlgebra::Vector3<ScalarType> &endT, ScalarType t, ::LinearAlgebra::Matrix4x4<ScalarType> &targetMem )
{
return OrientationMatrix( ::LinearAlgebra::Slerp(startR, endR, t), ::LinearAlgebra::Lerp(::LinearAlgebra::Vector4<ScalarType>(startT, (ScalarType)1.0f), ::LinearAlgebra::Vector4<ScalarType>(endT, (ScalarType)1.0f), t).xyz, targetMem );
}
}
#include "Utilities.h"

24
Code/OysterMath/Matrix.h
View File

@ -163,12 +163,18 @@ namespace LinearAlgebra
Vector4<ScalarType> GetRowVector( unsigned int rowID ) const;
const Vector4<ScalarType> & GetColumnVector( unsigned int colID ) const;
};
}
template<typename ScalarType> LinearAlgebra::Matrix2x2<ScalarType> operator * ( const ScalarType &left, const LinearAlgebra::Matrix2x2<ScalarType> &right );
template<typename ScalarType> LinearAlgebra::Matrix3x3<ScalarType> operator * ( const ScalarType &left, const LinearAlgebra::Matrix3x3<ScalarType> &right );
template<typename ScalarType> LinearAlgebra::Matrix4x4<ScalarType> operator * ( const ScalarType &left, const LinearAlgebra::Matrix4x4<ScalarType> &right );
///////////////////////////////////////////////////////////////////////////////////
// Body
///////////////////////////////////////////////////////////////////////////////////
namespace LinearAlgebra
{
// Matrix2x2<ScalarType> ///////////////////////////////////////
template<typename ScalarType>
@ -753,4 +759,22 @@ namespace LinearAlgebra
{ return this->v[colID]; }
}
template<typename ScalarType>
inline LinearAlgebra::Matrix2x2<ScalarType> operator * ( const ScalarType &left, const LinearAlgebra::Matrix2x2<ScalarType> &right )
{
return right * left;
}
template<typename ScalarType>
inline LinearAlgebra::Matrix3x3<ScalarType> operator * ( const ScalarType &left, const LinearAlgebra::Matrix3x3<ScalarType> &right )
{
return right * left;
}
template<typename ScalarType>
inline LinearAlgebra::Matrix4x4<ScalarType> operator * ( const ScalarType &left, const LinearAlgebra::Matrix4x4<ScalarType> &right )
{
return right * left;
}
#endif

24
Code/OysterMath/OysterMath.cpp
View File

@ -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 )
{

61
Code/OysterMath/OysterMath.h
View File

@ -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

180
Code/OysterMath/Vector.h
View File

@ -57,6 +57,12 @@ namespace LinearAlgebra
ScalarType GetMagnitude( ) const;
ScalarType Dot( const Vector2<ScalarType> &vector ) const;
//! @return (a.x * b.x, a.y * b.y)
Vector2<ScalarType> PiecewiseMultiplication( const Vector2<ScalarType> &vector ) const;
//! @return a = (a.x * b.x, a.y * b.y)
Vector2<ScalarType> & PiecewiseMultiplicationAdd( const Vector2<ScalarType> &vector );
Vector2<ScalarType> & Normalize( );
Vector2<ScalarType> GetNormalized( ) const;
};
@ -112,6 +118,12 @@ namespace LinearAlgebra
ScalarType Dot( const Vector3<ScalarType> &vector ) const;
Vector3<ScalarType> Cross( const Vector3<ScalarType> &vector ) const;
//! @return (a.x * b.x, a.y * b.y, a.z * b.z)
Vector3<ScalarType> PiecewiseMultiplication( const Vector3<ScalarType> &vector ) const;
//! @return a = (a.x * b.x, a.y * b.y, a.z * b.z)
Vector3<ScalarType> & PiecewiseMultiplicationAdd( const Vector3<ScalarType> &vector );
Vector3<ScalarType> & Normalize( );
Vector3<ScalarType> GetNormalized( ) const;
};
@ -169,14 +181,27 @@ namespace LinearAlgebra
ScalarType GetMagnitude( ) const;
ScalarType Dot( const Vector4<ScalarType> &vector ) const;
//! @return (a.x * b.x, a.y * b.y, a.z * b.z, a.w * b.w)
Vector4<ScalarType> PiecewiseMultiplication( const Vector4<ScalarType> &vector ) const;
//! @return a = (a.x * b.x, a.y * b.y, a.z * b.z, a.w * b.w)
Vector4<ScalarType> & PiecewiseMultiplicationAdd( const Vector4<ScalarType> &vector );
Vector4<ScalarType> & Normalize( );
Vector4<ScalarType> GetNormalized( ) const;
};
}
template<typename ScalarType> ::LinearAlgebra::Vector2<ScalarType> operator * ( const ScalarType &left, const ::LinearAlgebra::Vector2<ScalarType> &right );
template<typename ScalarType> ::LinearAlgebra::Vector3<ScalarType> operator * ( const ScalarType &left, const ::LinearAlgebra::Vector3<ScalarType> &right );
template<typename ScalarType> ::LinearAlgebra::Vector4<ScalarType> operator * ( const ScalarType &left, const ::LinearAlgebra::Vector4<ScalarType> &right );
///////////////////////////////////////////////////////////////////////////////////
// Body
///////////////////////////////////////////////////////////////////////////////////
namespace LinearAlgebra
{
// Vector2<ScalarType> ///////////////////////////////////////
template<typename ScalarType> const Vector2<ScalarType> Vector2<ScalarType>::null = Vector2<ScalarType>( 0, 0 );
@ -184,22 +209,22 @@ namespace LinearAlgebra
template<typename ScalarType> const Vector2<ScalarType> Vector2<ScalarType>::standard_unit_y = Vector2<ScalarType>( 0, 1 );
template<typename ScalarType>
Vector2<ScalarType>::Vector2( ) : x(), y() {}
inline Vector2<ScalarType>::Vector2( ) : x(), y() {}
template<typename ScalarType>
Vector2<ScalarType>::Vector2( const Vector2<ScalarType> &vector )
inline Vector2<ScalarType>::Vector2( const Vector2<ScalarType> &vector )
{ this->x = vector.x; this->y = vector.y; }
template<typename ScalarType>
Vector2<ScalarType>::Vector2( const ScalarType &_element )
inline Vector2<ScalarType>::Vector2( const ScalarType &_element )
{ this->x = this->y = _element; }
template<typename ScalarType>
Vector2<ScalarType>::Vector2( const ScalarType _element[2] )
inline Vector2<ScalarType>::Vector2( const ScalarType _element[2] )
{ this->x = _element[0]; this->y = _element[1]; }
template<typename ScalarType>
Vector2<ScalarType>::Vector2( const ScalarType &_x, const ScalarType &_y )
inline Vector2<ScalarType>::Vector2( const ScalarType &_x, const ScalarType &_y )
{ this->x = _x; this->y = _y; }
template<typename ScalarType>
@ -227,7 +252,7 @@ namespace LinearAlgebra
{ return this->element[i]; }
template<typename ScalarType>
Vector2<ScalarType> & Vector2<ScalarType>::operator = ( const Vector2<ScalarType> &vector )
inline Vector2<ScalarType> & Vector2<ScalarType>::operator = ( const Vector2<ScalarType> &vector )
{
this->element[0] = vector.element[0];
this->element[1] = vector.element[1];
@ -235,7 +260,7 @@ namespace LinearAlgebra
}
template<typename ScalarType>
Vector2<ScalarType> & Vector2<ScalarType>::operator = ( const ScalarType _element[2] )
inline Vector2<ScalarType> & Vector2<ScalarType>::operator = ( const ScalarType _element[2] )
{
this->element[0] = _element[0];
this->element[1] = _element[1];
@ -243,7 +268,7 @@ namespace LinearAlgebra
}
template<typename ScalarType>
Vector2<ScalarType> & Vector2<ScalarType>::operator *= ( const ScalarType &scalar )
inline Vector2<ScalarType> & Vector2<ScalarType>::operator *= ( const ScalarType &scalar )
{
this->element[0] *= scalar;
this->element[1] *= scalar;
@ -251,7 +276,7 @@ namespace LinearAlgebra
}
template<typename ScalarType>
Vector2<ScalarType> & Vector2<ScalarType>::operator /= ( const ScalarType &scalar )
inline Vector2<ScalarType> & Vector2<ScalarType>::operator /= ( const ScalarType &scalar )
{
this->element[0] /= scalar;
this->element[1] /= scalar;
@ -259,7 +284,7 @@ namespace LinearAlgebra
}
template<typename ScalarType>
Vector2<ScalarType> & Vector2<ScalarType>::operator += ( const Vector2<ScalarType> &vector )
inline Vector2<ScalarType> & Vector2<ScalarType>::operator += ( const Vector2<ScalarType> &vector )
{
this->element[0] += vector.element[0];
this->element[1] += vector.element[1];
@ -267,7 +292,7 @@ namespace LinearAlgebra
}
template<typename ScalarType>
Vector2<ScalarType> & Vector2<ScalarType>::operator -= ( const Vector2<ScalarType> &vector )
inline Vector2<ScalarType> & Vector2<ScalarType>::operator -= ( const Vector2<ScalarType> &vector )
{
this->element[0] -= vector.element[0];
this->element[1] -= vector.element[1];
@ -295,7 +320,7 @@ namespace LinearAlgebra
{ return Vector2<ScalarType>(-this->x, -this->y); }
template<typename ScalarType>
bool Vector2<ScalarType>::operator == ( const Vector2<ScalarType> &vector ) const
inline bool Vector2<ScalarType>::operator == ( const Vector2<ScalarType> &vector ) const
{
if( this->x != vector.x ) return false;
if( this->y != vector.y ) return false;
@ -303,7 +328,7 @@ namespace LinearAlgebra
}
template<typename ScalarType>
bool Vector2<ScalarType>::operator != ( const Vector2<ScalarType> &vector ) const
inline bool Vector2<ScalarType>::operator != ( const Vector2<ScalarType> &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<typename ScalarType>
ScalarType Vector2<ScalarType>::Dot( const Vector2<ScalarType> &vector ) const
inline ScalarType Vector2<ScalarType>::Dot( const Vector2<ScalarType> &vector ) const
{
ScalarType value = 0;
value += this->element[0] * vector.element[0];
@ -327,6 +352,20 @@ namespace LinearAlgebra
return value;
}
template<typename ScalarType>
inline Vector2<ScalarType> Vector2<ScalarType>::PiecewiseMultiplication( const Vector2<ScalarType> &vector ) const
{
return Vector2<ScalarType>( this->x * vector.x, this->y * vector.y );
}
template<typename ScalarType>
inline Vector2<ScalarType> & Vector2<ScalarType>::PiecewiseMultiplicationAdd( const Vector2<ScalarType> &vector )
{
this->x *= vector.x;
this->y *= vector.y;
return *this;
}
template<typename ScalarType>
inline Vector2<ScalarType> & Vector2<ScalarType>::Normalize( )
{ return (*this) /= this->GetLength(); }
@ -343,26 +382,26 @@ namespace LinearAlgebra
template<typename ScalarType> const Vector3<ScalarType> Vector3<ScalarType>::standard_unit_z = Vector3<ScalarType>( 0, 0, 1 );
template<typename ScalarType>
Vector3<ScalarType>::Vector3( ) : x(), y(), z() {}
inline Vector3<ScalarType>::Vector3( ) : x(), y(), z() {}
template<typename ScalarType>
Vector3<ScalarType>::Vector3( const Vector3<ScalarType> &vector )
inline Vector3<ScalarType>::Vector3( const Vector3<ScalarType> &vector )
{ this->x = vector.x; this->y = vector.y; this->z = vector.z; }
template<typename ScalarType>
Vector3<ScalarType>::Vector3( const Vector2<ScalarType> &vector, const ScalarType &_z )
inline Vector3<ScalarType>::Vector3( const Vector2<ScalarType> &vector, const ScalarType &_z )
{ this->x = vector.x; this->y = vector.y; this->z = _z; }
template<typename ScalarType>
Vector3<ScalarType>::Vector3( const ScalarType &_element )
inline Vector3<ScalarType>::Vector3( const ScalarType &_element )
{ this->x = this->y = this->z = _element; }
template<typename ScalarType>
Vector3<ScalarType>::Vector3( const ScalarType _element[3] )
inline Vector3<ScalarType>::Vector3( const ScalarType _element[3] )
{ this->x = _element[0]; this->y = _element[1]; this->z = _element[2]; }
template<typename ScalarType>
Vector3<ScalarType>::Vector3( const ScalarType &_x, const ScalarType &_y, const ScalarType &_z )
inline Vector3<ScalarType>::Vector3( const ScalarType &_x, const ScalarType &_y, const ScalarType &_z )
{ this->x = _x; this->y = _y; this->z = _z; }
template<typename ScalarType>
@ -382,7 +421,7 @@ namespace LinearAlgebra
{ return this->element[i]; }
template<typename ScalarType>
Vector3<ScalarType> & Vector3<ScalarType>::operator = ( const Vector3<ScalarType> &vector )
inline Vector3<ScalarType> & Vector3<ScalarType>::operator = ( const Vector3<ScalarType> &vector )
{
this->element[0] = vector.element[0];
this->element[1] = vector.element[1];
@ -391,7 +430,7 @@ namespace LinearAlgebra
}
template<typename ScalarType>
Vector3<ScalarType> & Vector3<ScalarType>::operator = ( const ScalarType element[3] )
inline Vector3<ScalarType> & Vector3<ScalarType>::operator = ( const ScalarType element[3] )
{
this->element[0] = element[0];
this->element[1] = element[1];
@ -400,7 +439,7 @@ namespace LinearAlgebra
}
template<typename ScalarType>
Vector3<ScalarType> & Vector3<ScalarType>::operator *= ( const ScalarType &scalar )
inline Vector3<ScalarType> & Vector3<ScalarType>::operator *= ( const ScalarType &scalar )
{
this->element[0] *= scalar;
this->element[1] *= scalar;
@ -409,7 +448,7 @@ namespace LinearAlgebra
}
template<typename ScalarType>
Vector3<ScalarType> & Vector3<ScalarType>::operator /= ( const ScalarType &scalar )
inline Vector3<ScalarType> & Vector3<ScalarType>::operator /= ( const ScalarType &scalar )
{
this->element[0] /= scalar;
this->element[1] /= scalar;
@ -418,7 +457,7 @@ namespace LinearAlgebra
}
template<typename ScalarType>
Vector3<ScalarType> & Vector3<ScalarType>::operator += ( const Vector3<ScalarType> &vector )
inline Vector3<ScalarType> & Vector3<ScalarType>::operator += ( const Vector3<ScalarType> &vector )
{
this->element[0] += vector.element[0];
this->element[1] += vector.element[1];
@ -427,7 +466,7 @@ namespace LinearAlgebra
}
template<typename ScalarType>
Vector3<ScalarType> & Vector3<ScalarType>::operator -= ( const Vector3<ScalarType> &vector )
inline Vector3<ScalarType> & Vector3<ScalarType>::operator -= ( const Vector3<ScalarType> &vector )
{
this->element[0] -= vector.element[0];
this->element[1] -= vector.element[1];
@ -456,7 +495,7 @@ namespace LinearAlgebra
{ return Vector3<ScalarType>(-this->x, -this->y, -this->z); }
template<typename ScalarType>
bool Vector3<ScalarType>::operator == ( const Vector3<ScalarType> &vector ) const
inline bool Vector3<ScalarType>::operator == ( const Vector3<ScalarType> &vector ) const
{
if( this->x != vector.x ) return false;
if( this->y != vector.y ) return false;
@ -465,7 +504,7 @@ namespace LinearAlgebra
}
template<typename ScalarType>
bool Vector3<ScalarType>::operator != ( const Vector3<ScalarType> &vector ) const
inline bool Vector3<ScalarType>::operator != ( const Vector3<ScalarType> &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<typename ScalarType>
ScalarType Vector3<ScalarType>::Dot( const Vector3<ScalarType> &vector ) const
inline ScalarType Vector3<ScalarType>::Dot( const Vector3<ScalarType> &vector ) const
{
ScalarType value = 0;
value += this->element[0] * vector.element[0];
@ -492,13 +531,28 @@ namespace LinearAlgebra
}
template<typename ScalarType>
Vector3<ScalarType> Vector3<ScalarType>::Cross( const Vector3<ScalarType> &vector ) const
inline Vector3<ScalarType> Vector3<ScalarType>::Cross( const Vector3<ScalarType> &vector ) const
{
return Vector3<ScalarType>( (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<typename ScalarType>
inline Vector3<ScalarType> Vector3<ScalarType>::PiecewiseMultiplication( const Vector3<ScalarType> &vector ) const
{
return Vector3<ScalarType>( this->x * vector.x, this->y * vector.y, this->z * vector.z );
}
template<typename ScalarType>
inline Vector3<ScalarType> & Vector3<ScalarType>::PiecewiseMultiplicationAdd( const Vector3<ScalarType> &vector )
{
this->x *= vector.x;
this->y *= vector.y;
this->z *= vector.z;
return *this;
}
template<typename ScalarType>
inline Vector3<ScalarType> & Vector3<ScalarType>::Normalize( )
{ return (*this) /= this->GetLength(); }
@ -516,30 +570,30 @@ namespace LinearAlgebra
template<typename ScalarType> const Vector4<ScalarType> Vector4<ScalarType>::standard_unit_w = Vector4<ScalarType>( 0, 0, 0, 1 );
template<typename ScalarType>
Vector4<ScalarType>::Vector4( ) : x(), y(), z(), w() {}
inline Vector4<ScalarType>::Vector4( ) : x(), y(), z(), w() {}
template<typename ScalarType>
Vector4<ScalarType>::Vector4( const Vector4<ScalarType> &vector )
inline Vector4<ScalarType>::Vector4( const Vector4<ScalarType> &vector )
{ this->x = vector.x; this->y = vector.y; this->z = vector.z; this->w = vector.w; }
template<typename ScalarType>
Vector4<ScalarType>::Vector4( const Vector3<ScalarType> &vector, const ScalarType &_w )
inline Vector4<ScalarType>::Vector4( const Vector3<ScalarType> &vector, const ScalarType &_w )
{ this->x = vector.x; this->y = vector.y; this->z = vector.z; this->w = _w; }
template<typename ScalarType>
Vector4<ScalarType>::Vector4( const Vector2<ScalarType> &vector, const ScalarType &_z, const ScalarType &_w )
inline Vector4<ScalarType>::Vector4( const Vector2<ScalarType> &vector, const ScalarType &_z, const ScalarType &_w )
{ this->x = vector.x; this->y = vector.y; this->z = _z; this->w = _w; }
template<typename ScalarType>
Vector4<ScalarType>::Vector4( const ScalarType &_element )
inline Vector4<ScalarType>::Vector4( const ScalarType &_element )
{ this->x = this->y = this->z = this->w = _element; }
template<typename ScalarType>
Vector4<ScalarType>::Vector4( const ScalarType _element[4] )
inline Vector4<ScalarType>::Vector4( const ScalarType _element[4] )
{ this->x = _element[0]; this->y = _element[1]; this->z = _element[2]; this->w = _element[3]; }
template<typename ScalarType>
Vector4<ScalarType>::Vector4( const ScalarType &_x, const ScalarType &_y, const ScalarType &_z, const ScalarType &_w )
inline Vector4<ScalarType>::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<typename ScalarType>
@ -559,7 +613,7 @@ namespace LinearAlgebra
{ return this->element[i]; }
template<typename ScalarType>
Vector4<ScalarType> & Vector4<ScalarType>::operator = ( const Vector4<ScalarType> &vector )
inline Vector4<ScalarType> & Vector4<ScalarType>::operator = ( const Vector4<ScalarType> &vector )
{
this->element[0] = vector.element[0];
this->element[1] = vector.element[1];
@ -569,7 +623,7 @@ namespace LinearAlgebra
}
template<typename ScalarType>
Vector4<ScalarType> & Vector4<ScalarType>::operator = ( const ScalarType element[4] )
inline Vector4<ScalarType> & Vector4<ScalarType>::operator = ( const ScalarType element[4] )
{
this->element[0] = element[0];
this->element[1] = element[1];
@ -579,7 +633,7 @@ namespace LinearAlgebra
}
template<typename ScalarType>
Vector4<ScalarType> & Vector4<ScalarType>::operator *= ( const ScalarType &scalar )
inline Vector4<ScalarType> & Vector4<ScalarType>::operator *= ( const ScalarType &scalar )
{
this->element[0] *= scalar;
this->element[1] *= scalar;
@ -589,7 +643,7 @@ namespace LinearAlgebra
}
template<typename ScalarType>
Vector4<ScalarType> & Vector4<ScalarType>::operator /= ( const ScalarType &scalar )
inline Vector4<ScalarType> & Vector4<ScalarType>::operator /= ( const ScalarType &scalar )
{
this->element[0] /= scalar;
this->element[1] /= scalar;
@ -599,7 +653,7 @@ namespace LinearAlgebra
}
template<typename ScalarType>
Vector4<ScalarType> & Vector4<ScalarType>::operator += ( const Vector4<ScalarType> &vector )
inline Vector4<ScalarType> & Vector4<ScalarType>::operator += ( const Vector4<ScalarType> &vector )
{
this->element[0] += vector.element[0];
this->element[1] += vector.element[1];
@ -609,7 +663,7 @@ namespace LinearAlgebra
}
template<typename ScalarType>
Vector4<ScalarType> & Vector4<ScalarType>::operator -= ( const Vector4<ScalarType> &vector )
inline Vector4<ScalarType> & Vector4<ScalarType>::operator -= ( const Vector4<ScalarType> &vector )
{
this->element[0] -= vector.element[0];
this->element[1] -= vector.element[1];
@ -639,7 +693,7 @@ namespace LinearAlgebra
{ return Vector4<ScalarType>(-this->x, -this->y, -this->z, -this->w); }
template<typename ScalarType>
bool Vector4<ScalarType>::operator == ( const Vector4<ScalarType> &vector ) const
inline bool Vector4<ScalarType>::operator == ( const Vector4<ScalarType> &vector ) const
{
if( this->x != vector.x ) return false;
if( this->y != vector.y ) return false;
@ -649,7 +703,7 @@ namespace LinearAlgebra
}
template<typename ScalarType>
bool Vector4<ScalarType>::operator != ( const Vector4<ScalarType> &vector ) const
inline bool Vector4<ScalarType>::operator != ( const Vector4<ScalarType> &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<typename ScalarType>
ScalarType Vector4<ScalarType>::Dot( const Vector4<ScalarType> &vector ) const
inline ScalarType Vector4<ScalarType>::Dot( const Vector4<ScalarType> &vector ) const
{
ScalarType value = 0;
value += this->element[0] * vector.element[0];
@ -677,6 +731,22 @@ namespace LinearAlgebra
return value;
}
template<typename ScalarType>
inline Vector4<ScalarType> Vector4<ScalarType>::PiecewiseMultiplication( const Vector4<ScalarType> &vector ) const
{
return Vector4<ScalarType>( this->x * vector.x, this->y * vector.y, this->z * vector.z, this->w * vector.w );
}
template<typename ScalarType>
inline Vector4<ScalarType> & Vector4<ScalarType>::PiecewiseMultiplicationAdd( const Vector4<ScalarType> &vector )
{
this->x *= vector.x;
this->y *= vector.y;
this->z *= vector.z;
this->w *= vector.w;
return *this;
}
template<typename ScalarType>
inline Vector4<ScalarType> & Vector4<ScalarType>::Normalize( )
{ return (*this) /= this->GetLength(); }
@ -686,4 +756,22 @@ namespace LinearAlgebra
{ return Vector4<ScalarType>(*this).Normalize(); }
}
template<typename ScalarType>
inline ::LinearAlgebra::Vector2<ScalarType> operator * ( const ScalarType &left, const ::LinearAlgebra::Vector2<ScalarType> &right )
{
return right * left;
}
template<typename ScalarType>
inline ::LinearAlgebra::Vector3<ScalarType> operator * ( const ScalarType &left, const ::LinearAlgebra::Vector3<ScalarType> &right )
{
return right * left;
}
template<typename ScalarType>
inline ::LinearAlgebra::Vector4<ScalarType> operator * ( const ScalarType &left, const ::LinearAlgebra::Vector4<ScalarType> &right )
{
return right * left;
}
#endif

5
Code/OysterPhysics3D/Frustrum.cpp
View File

@ -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;
}

2
Code/OysterPhysics3D/Frustrum.h
View File

@ -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 ///////////////////////////////////////

51
Code/OysterPhysics3D/Inertia.cpp Normal file
View File

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

119
Code/OysterPhysics3D/Inertia.h Normal file
View File

@ -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

2
Code/OysterPhysics3D/OysterPhysics3D.vcxproj
View File

@ -155,6 +155,7 @@
<ClInclude Include="FluidDrag.h" />
<ClInclude Include="Frustrum.h" />
<ClInclude Include="ICollideable.h" />
<ClInclude Include="Inertia.h" />
<ClInclude Include="Line.h" />
<ClInclude Include="OysterCollision3D.h" />
<ClInclude Include="OysterPhysics3D.h" />
@ -174,6 +175,7 @@
<ClCompile Include="FluidDrag.cpp" />
<ClCompile Include="Frustrum.cpp" />
<ClCompile Include="ICollideable.cpp" />
<ClCompile Include="Inertia.cpp" />
<ClCompile Include="Line.cpp" />
<ClCompile Include="OysterCollision3D.cpp" />
<ClCompile Include="Particle.cpp" />

6
Code/OysterPhysics3D/OysterPhysics3D.vcxproj.filters
View File

@ -78,6 +78,9 @@
<ClInclude Include="RigidBody_Inline.h">
<Filter>Header Files\Physics</Filter>
</ClInclude>
<ClInclude Include="Inertia.h">
<Filter>Header Files\Physics</Filter>
</ClInclude>
</ItemGroup>
<ItemGroup>
<ClCompile Include="Box.cpp">
@ -125,5 +128,8 @@
<ClCompile Include="Particle.cpp">
<Filter>Source Files\Physics</Filter>
</ClCompile>
<ClCompile Include="Inertia.cpp">
<Filter>Source Files\Physics</Filter>
</ClCompile>
</ItemGroup>
</Project>

108
Code/OysterPhysics3D/RigidBody.cpp
View File

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

68
Code/OysterPhysics3D/RigidBody.h
View File

@ -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.
};
} }

29
Code/OysterPhysics3D/RigidBody_Inline.h
View File

@ -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