Encapsulates a rigid body. More...

#include <RigidBody.h>

Inheritance diagram for WoRB::RigidBody:

Collaboration diagram for WoRB::RigidBody:

Public Member Functions
Constructors and destructors
	RigidBody ()
Integration and simulation methods
These methods are used to simulate the rigid body's motion over time.
void	SolveODE (double h)
	Integrates the rigid body forward in time for the given time-step length.
void	CalculateDerivedQuantities (bool fromMomenta=true)
	Normalizes orientation and calculates derived quantities from the state variables.
void	DampMomentum (double timeStep)
	Damp linear and angular momentum.
Property getters and setters
void	SetupMass (double mass)
	Sets the mass of the rigid body.
double	Mass () const
	Gets the mass of the rigid body.
bool	IsFiniteMass () const
	Returns true if the mass of the body is not-infinite.
void	Set_XQVW (const Quaternion &X, const Quaternion &Q, const Quaternion &V, const Quaternion &W)
	Initializs the position, orientation, velocity and angular velocity of the rigid body and updates other derived quantities.
void	SetMomentOfInertia (const QTensor &I_body)
	Sets the intertia tensor for the rigid body.
Setters for the force, torque and acceleration
These functions set up forces and torques to apply to the rigid body.
void	ClearAccumulators ()
	Clears the forces and torques in the accumulators.
void	AddExternalForce (const Quaternion &force, double potentialEnergy=0)
	Adds the given external force to center of mass of the rigid body.
void	AddForce (const Quaternion &force, double potentialEnergy=0)
	Adds the given internal force to center of mass of the rigid body.
void	AddForceAtPoint (const Quaternion &worldPoint, const Quaternion &force, double potentialEnergy=0)
	Adds the given internal force to the given point on the rigid body.
void	AddForceAtBodyPoint (const Quaternion &bodyPoint, const Quaternion &force, double potentialEnergy=0)
	Adds the given internal force to the given point on the rigid body.
void	AddTorque (const Quaternion &torque)
	Adds the given internal torque to the rigid body.
Data Fields
Properties and state variables of the rigid body
double	InverseMass
	Holds the inverse of the mass of the rigid body.
QTensor	InverseInertiaBody
	Holds the inverse of the body's moment of inertia tensor in body-fixed frame.
Quaternion	Position
	Holds the linear position of the rigid body in world space.
Quaternion	Orientation
	Holds the angular orientation of the rigid body in world space.
Quaternion	LinearMomentum
	Holds the linear momentum of the rigid body in world space.
Quaternion	AngularMomentum
	Holds the angular momentum of the rigid body in world space.
Derived quantities
QTensor	ToWorld
	Holds a combined translation/rotation matrix for converting from body-fixed local coordinates into world coordinates.
QTensor	InverseInertiaWorld
	Holds the inverse inertia tensor of the body in world space.
Quaternion	Velocity
	Holds the linear velocity of the rigid body in world space.
Quaternion	AngularVelocity
	Holds the angular velocity of the rigid body in world space.
Quaternion	TotalAngularMomentum
	Holds the total angular momentum of the rigid body in world space.
double	KineticEnergy
	Holds the total kinetic energy of the rigid body.
double	PotentialEnergy
	Holds the total potential energy of the rigid body.
double	AverageKineticEnergy
	Holds the amount of weighted mean kinetic energy of the body.
double	KineticEnergyThreshold
	Holds the kinetic energy level under which a body will considered stationary.
bool	KineticEnergyDamping
	Indicates whether to impose kinetic energy damping.
Total force and torque accumulators
These variables store the current total force, torque and acceleration of the rigid body; these quantities are accummulated inbetween time-steps.
Quaternion	Force
	Holds the accumulated force to be applied at the next integration step.
Quaternion	Torque
	Holds the accumulated torque to be applied at the next integration step.
Flags controlling ODE solver activity for the body
bool	IsActive
	A body can be inactivated to avoid it being updated SolveODE or affected by collisions with the scenery.
bool	CanBeDeactivated
	Controls whether body is allowed to be inactivated.
void	Activate ()
	Allows body to move, i.e.
void	Deactivate ()
	Disallows body to move i.e.
void	SetCanBeDeactivated (bool flag=true)
	Sets whether the body is ever allowed to be deactivated.

Detailed Description

Encapsulates a rigid body.

Rigid body is the basic simulation object in the World of Bodies (WoRB).

Properties:

inverse mass, and
inverse moment of inertia

State variables:

position,
orientation,
linear momentum, and
angular momentum

Derived quantities:

linear and angular velocity, kinetic energy, etc.

Definition at line 36 of file RigidBody.h.

Constructor & Destructor Documentation

WoRB::RigidBody::RigidBody ( ) [inline]

Definition at line 132 of file RigidBody.h.

            : InverseMass( 0 )
            , ToWorld( QTensor::Identity )
            , KineticEnergy( 0 )
            , PotentialEnergy( 0 )
            , AverageKineticEnergy( 0 )
            , KineticEnergyThreshold( 0 )
            , KineticEnergyDamping( false )
            , IsActive( false )
            , CanBeDeactivated( false )
        {
        }

Member Function Documentation

void WoRB::RigidBody::Activate ( ) [inline]

Allows body to move, i.e.

enables its ODE to be solved.

Definition at line 343 of file RigidBody.h.

References AverageKineticEnergy, IsActive, and Mass().

Referenced by WoRB::Collision::ActivateInactiveBodies(), WoRB::Ball::Ball(), WoRB::Box::Box(), and SetCanBeDeactivated().

        {
            if ( ! IsActive ) 
            {
                IsActive = true;
                // Body must have some kinetic energy to avoid immediate deactivation
                AverageKineticEnergy = 2 * 0.3 * Mass(); 
            }
        }

void WoRB::RigidBody::AddExternalForce	(	const Quaternion &	force,
		double	potentialEnergy = `0`
	)		`[inline]`

Adds the given external force to center of mass of the rigid body.

An external force acts on the whole body equally and does not activate inactive bodies.

Definition at line 399 of file RigidBody.h.

References Force, and PotentialEnergy.

        {
            Force += force;
            PotentialEnergy += potentialEnergy;
        }

void WoRB::RigidBody::AddForce	(	const Quaternion &	force,
		double	potentialEnergy = `0`
	)		`[inline]`

Adds the given internal force to center of mass of the rigid body.

Definition at line 407 of file RigidBody.h.

References Force, IsActive, and PotentialEnergy.

        {
            Force += force;
            PotentialEnergy += potentialEnergy;
            IsActive = true;
        }

void WoRB::RigidBody::AddForceAtBodyPoint	(	const Quaternion &	bodyPoint,
		const Quaternion &	force,
		double	potentialEnergy = `0`
	)		`[inline]`

Adds the given internal force to the given point on the rigid body.

Definition at line 427 of file RigidBody.h.

References AddForceAtPoint(), and ToWorld.

        {
            AddForceAtPoint( ToWorld( bodyPoint ), force, potentialEnergy );
        }

void WoRB::RigidBody::AddForceAtPoint	(	const Quaternion &	worldPoint,
		const Quaternion &	force,
		double	potentialEnergy = `0`
	)		`[inline]`

Adds the given internal force to the given point on the rigid body.

Definition at line 416 of file RigidBody.h.

References Force, IsActive, Position, PotentialEnergy, and Torque.

Referenced by AddForceAtBodyPoint().

        {
            Force += force;
            Torque += ( worldPoint - Position ).Cross( force );
            PotentialEnergy += potentialEnergy;
            IsActive = true;
        }

void WoRB::RigidBody::AddTorque ( const Quaternion & torque ) [inline]

Adds the given internal torque to the rigid body.

Definition at line 435 of file RigidBody.h.

References IsActive, and Torque.

        {
            Torque += torque;
            IsActive = true;
        }

void WoRB::RigidBody::CalculateDerivedQuantities ( bool fromMomenta = true ) [inline]

Normalizes orientation and calculates derived quantities from the state variables.

Parameters:

fromMomenta If true, derive quantites from linear and angular momenta; otherwise, derive quantities from velocities.

Definition at line 221 of file RigidBody.h.

References AngularMomentum, AngularVelocity, WoRB::Quaternion::Cross(), WoRB::Quaternion::Dot(), WoRB::QTensor::Inverse(), InverseInertiaBody, InverseInertiaWorld, InverseMass, KineticEnergy, LinearMomentum, Mass(), WoRB::Quaternion::Normalize(), Orientation, Position, WoRB::QTensor::SetFromOrientationAndPosition(), TotalAngularMomentum, ToWorld, and Velocity.

Referenced by WoRB::Collision::PositionProjection(), WoRB_TestBed::ReconfigureTestBed(), Set_XQVW(), WoRB::Sphere::SetMass(), WoRB::Cuboid::SetMass(), and SolveODE().

        {
            // Normalize the orientation to versor
            //
            Orientation.Normalize ();

            // Setup the transform matrix for the body from orientation and position.
            //
            ToWorld.SetFromOrientationAndPosition( Orientation, Position );

            // Setup the moment of inertia tensor in world frame of reference.
            //
            InverseInertiaWorld = ToWorld( InverseInertiaBody );

            if ( fromMomenta )
            {
                // Calculate the linear and angular velocity from respecitve momenta
                //
                Velocity = InverseMass * LinearMomentum;
                AngularVelocity = InverseInertiaWorld * AngularMomentum;
            }
            else
            {
                // Calculate the linear and angular momentum from repsective velocities
                //
                LinearMomentum  = Mass()  * Velocity;
                AngularMomentum = InverseInertiaWorld.Inverse () * AngularVelocity;
            }

            // Calculate the total angular momentum
            //
            TotalAngularMomentum = Position.Cross( LinearMomentum ) + AngularMomentum;

            // Derive the total kinetic energy
            //
            KineticEnergy = 0.5 * Velocity        .Dot( LinearMomentum  )
                          + 0.5 * AngularVelocity .Dot( AngularMomentum );
        }

void WoRB::RigidBody::ClearAccumulators ( ) [inline]

Clears the forces and torques in the accumulators.

Definition at line 388 of file RigidBody.h.

References Force, PotentialEnergy, and Torque.

        {
            Force  = 0;
            Torque = 0;
            PotentialEnergy = 0;
        }

void WoRB::RigidBody::DampMomentum ( double timeStep ) [inline]

Damp linear and angular momentum.

Definition at line 262 of file RigidBody.h.

References AngularMomentum, and LinearMomentum.

Referenced by SolveODE().

        {
            const double linearDamping  = 0;
            const double angularDamping = 0.998;

            if ( linearDamping > 0 ) {
                LinearMomentum *= pow( linearDamping, timeStep );
            }
            if ( angularDamping > 0 ) {
                AngularMomentum *= pow( angularDamping, timeStep );
            }
        }

void WoRB::RigidBody::Deactivate ( ) [inline]

Disallows body to move i.e.

disables its ODE to be solved. For deativated body, its velocities are also set to 0.

Definition at line 356 of file RigidBody.h.

References AngularMomentum, AngularVelocity, Force, IsActive, KineticEnergy, LinearMomentum, Torque, TotalAngularMomentum, and Velocity.

Referenced by WoRB_MexFunction::Parse(), and SolveODE().

        {
            IsActive             = false;
            LinearMomentum       = 0.0;
            AngularMomentum      = 0.0;
            TotalAngularMomentum = 0.0;
            Velocity             = 0;
            AngularVelocity      = 0;
            KineticEnergy        = 0;
            Force                = 0.0;
            Torque               = 0.0;
        }

bool WoRB::RigidBody::IsFiniteMass ( ) const [inline]

Returns true if the mass of the body is not-infinite.

Definition at line 298 of file RigidBody.h.

References InverseMass.

        {
            return InverseMass > 0.0;
        }

double WoRB::RigidBody::Mass ( ) const [inline]

Gets the mass of the rigid body.

Definition at line 290 of file RigidBody.h.

References InverseMass.

Referenced by Activate(), CalculateDerivedQuantities(), and WoRB_TestBed::Dump().

        {
            return InverseMass == 0 ? 1e+30 
                 : InverseMass >= 1e+30 ? 0.0 : 1.0 / InverseMass;
        }

void WoRB::RigidBody::Set_XQVW	(	const Quaternion &	X,
		const Quaternion &	Q,
		const Quaternion &	V,
		const Quaternion &	W
	)		`[inline]`

Initializs the position, orientation, velocity and angular velocity of the rigid body and updates other derived quantities.

Parameters:

X	The initial position
Q	The initial orientation
V	The initial velocity
W	The initial angular velocity in world space

Definition at line 307 of file RigidBody.h.

References AngularVelocity, CalculateDerivedQuantities(), Orientation, Position, and Velocity.

Referenced by WoRB::Ball::Ball(), and WoRB::Box::Box().

        {
            Position        = X;
            Orientation     = Q;
            Velocity        = V;
            AngularVelocity = W;

            CalculateDerivedQuantities( /*fromMomenta*/ false );
        }

void WoRB::RigidBody::SetCanBeDeactivated ( bool flag = true ) [inline]

Sets whether the body is ever allowed to be deactivated.

Definition at line 371 of file RigidBody.h.

References Activate(), CanBeDeactivated, and IsActive.

Referenced by WoRB_MexFunction::Parse().

        {
            CanBeDeactivated = flag;

            if ( ! CanBeDeactivated && ! IsActive ) {
                Activate ();
            }
        }

void WoRB::RigidBody::SetMomentOfInertia ( const QTensor & I_body ) [inline]

Sets the intertia tensor for the rigid body.

Definition at line 324 of file RigidBody.h.

References InverseInertiaBody, and WoRB::QTensor::SetInverseOf().

Referenced by WoRB::Sphere::SetMass(), and WoRB::Cuboid::SetMass().

        {
            InverseInertiaBody.SetInverseOf( I_body );
        }

void WoRB::RigidBody::SetupMass ( double mass ) [inline]

Sets the mass of the rigid body.

Definition at line 280 of file RigidBody.h.

References InverseMass, and KineticEnergyThreshold.

Referenced by WoRB::Sphere::SetMass(), and WoRB::Cuboid::SetMass().

        {
            InverseMass = mass == 0 ? 1e+30 
                        : mass >= 1e+30 ? 0.0 : 1.0 / mass;

            KineticEnergyThreshold = 0.3 * mass;
        }

void WoRB::RigidBody::SolveODE ( double h ) [inline]

Integrates the rigid body forward in time for the given time-step length.

Definition at line 153 of file RigidBody.h.

References AngularMomentum, AngularVelocity, AverageKineticEnergy, CalculateDerivedQuantities(), CanBeDeactivated, DampMomentum(), Deactivate(), Force, InverseInertiaWorld, InverseMass, IsActive, KineticEnergy, KineticEnergyDamping, KineticEnergyThreshold, LinearMomentum, Orientation, Position, Torque, and Velocity.

        {
            if ( ! IsActive ) {
                return;
            }

            // Solve the linear momentum
            //
            LinearMomentum += Force * h;

            // Solve the angular momentum in world space (included gyroscopic effect)
            //
            AngularMomentum += Torque * h;

            // If enabled, remove kinetic energy added through the numerical 
            // instability in the Semi-implicit Euler integrator.
            //
            if ( KineticEnergyDamping ) {
                DampMomentum( h );
            }

            // Derive the linear and the angular velocity
            //
            Velocity = InverseMass * LinearMomentum;
            AngularVelocity = InverseInertiaWorld * AngularMomentum;

            // Calculate the orientation time derivative in world space
            //
            Quaternion OrientationDot = 0.5 * AngularVelocity * Orientation;

            // Solve the linear position
            //
            Position += Velocity * h;

            // Solve the orientation (angular position)
            //
            Orientation += OrientationDot * h;

            // Normalize orientation to versor and calculate derived quantities
            //
            CalculateDerivedQuantities ();

            // Deactivate body, if allowed, when it becomes stationary.
            //
            if ( CanBeDeactivated )
            {
                // Calculate exponential average of the kinetic energy
                //
                double alpha = pow( 0.5, h ); // alpha = 1 / 2^h
                AverageKineticEnergy = alpha * AverageKineticEnergy 
                                     + ( 1 - alpha ) * KineticEnergy;

                if ( AverageKineticEnergy < KineticEnergyThreshold ) {
                    Deactivate ();
                }
                else if ( AverageKineticEnergy > 10 * KineticEnergyThreshold ) {
                    AverageKineticEnergy = 10 * KineticEnergyThreshold;
                }
            }
        }