GL - merge with physics

This commit is contained in:
lindaandersson 2014-01-28 15:43:32 +01:00
commit 9ccfebd9fd
28 changed files with 1753 additions and 374 deletions

Binary file not shown.

Binary file not shown.

File diff suppressed because it is too large Load Diff

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

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

View File

@ -31,8 +31,8 @@ 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 );
@ -40,7 +40,7 @@ namespace
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 )

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

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 )

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 )

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.

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 );
::Oyster::Math::Quaternion rotation = ::Oyster::Math3D::Rotation(this->angularAxis);
::Oyster::Math::Float3 w = this->inertiaTensor.CalculateAngularVelocity( rotation, this->angularMomentum );
this->inertiaTensor = tensor;
this->angularMomentum = ::Oyster::Physics3D::Formula::AngularMomentum( rotation * tensor, w );
}
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;

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

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"

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

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

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

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

View File

@ -242,3 +242,8 @@ bool Frustrum::Contains( const ICollideable &target ) const
default: return false;
}
}
::Oyster::Math::Float3 Frustrum::ExtractForwad()
{
return this->bottomPlane.normal.xyz;
}

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

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

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

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

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>

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;
}
@ -53,15 +53,16 @@ void RigidBody::Update_LeapFrog( Float updateFrameLength )
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 );
Float3 w = this->momentOfInertiaTensor.CalculateAngularVelocity( this->rotation, this->momentum_Angular );
this->momentOfInertiaTensor = localTensorI;
this->momentum_Angular = Formula::AngularMomentum( rotationMatrix * localTensorI, w );
}
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;
}
}
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;
Float3 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) );
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 );
}

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;
const ::Oyster::Physics3D::MomentOfInertia & GetMomentOfInertia() const;
::Oyster::Math::Float GetMass() const;
const ::Oyster::Math::Quaternion & GetRotation() 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::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;
::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.
};
} }

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