Encapsulates collision response framework. More...

#include <CollisionResolver.h>

Collaboration diagram for WoRB::CollisionResolver:

[legend]

Public Member Functions
Constructor
	CollisionResolver (Collision *allocationArea, unsigned length)
	Instantiates a collision detection framework in the given area.
Property getters and index operations
unsigned	Count () const
	Gets number of contats in the registry.
bool	HasSpaceForMoreContacts () const
	Checks if there is a free space available for more contact data.
const Collision &	operator[] (unsigned index) const
	Gets collision data with the specified index.
Collision detection methods
void	Initialize ()
	Clears the collision registry.
unsigned	RegisterNewContact (RigidBody body_A, RigidBody body_B, const Quaternion &position, const Quaternion &normal, double penetration)
	Registers a new contact.
void	UpdateDerivedQuantities (double timeStep)
	Updates drived quantities (like contact velocity and axis info).
Collision response methods
void	ImpulseTransfers (double timeStep, unsigned maxIterations=0, double velocityEPS=0.01)
	Resolves collisions using the impulse transfer method.
void	PositionProjections (unsigned maxIterations=0, double positionEPS=0.01)
	Resolves collisions using the position projection method.
Miscellanous methods
void	Dump (double currentTime) const
	Displays information about all registered collisions on standard output.
Collision *	FindLargestBouncingVelocity (double eps)
	Find a collision with the maximum bouncing velocity.
Collision *	FindLargestPenetration (double eps)
	Finds a collision with the largest penetration.
Data Fields
Public properties
double	Restitution
	Holds the restitution coefficient common for all collisions.
double	Relaxation
	Holds the position projections coefficient common for all collisions.
double	Friction
	Holds the friction coefficient common for all collisions.
Private Attributes
Private properties
unsigned	MaxCollisionCount
	Holds the maximum number of collisions the array can take.
Collision *	Collisions
	Holds the parcel for the collision data.
Collision *	NextFree
	Holds the next free place in the parcel.
unsigned	FreeCount
	Holds the free space in allocation area.
unsigned	CollisionCount
	Holds the current number of collisions found.

Detailed Description

Encapsulates collision response framework.

All collisions in the system, after detection, are registered in an instance of the CollisionResolver class and then resolved using the impulse transfer and the position projections methods.

The CollisionResolver instance may be shared between different WoRB systems.

Definition at line 27 of file CollisionResolver.h.

Constructor & Destructor Documentation

WoRB::CollisionResolver::CollisionResolver	(	Collision *	allocationArea,
		unsigned	length
	)		`[inline]`

Instantiates a collision detection framework in the given area.

Definition at line 75 of file CollisionResolver.h.

            : MaxCollisionCount( length )
            , Collisions( allocationArea )
            , NextFree( allocationArea )
            , FreeCount( length )
            , CollisionCount( 0 )
            , Restitution( 1.0 )
            , Relaxation( 0.2 )
            , Friction( 0.0 )
        {
        }

Member Function Documentation

unsigned WoRB::CollisionResolver::Count ( ) const [inline]

Gets number of contats in the registry.

Definition at line 92 of file CollisionResolver.h.

References CollisionCount.

Referenced by WoRB_MexFunction::OnProcessData().

        {
            return CollisionCount;
        }

void CollisionResolver::Dump ( double currentTime ) const

Displays information about all registered collisions on standard output.

Definition at line 43 of file WoRB.cpp.

References CollisionCount, Collisions, and WoRB::Collision::Dump().

{
    for ( unsigned i = 0; i < CollisionCount; ++i )
    {
        Collisions[i].Dump( i, currentTime );
    }
}

Collision* WoRB::CollisionResolver::FindLargestBouncingVelocity ( double eps ) [inline]

Find a collision with the maximum bouncing velocity.

Definition at line 189 of file CollisionResolver.h.

References WoRB::Collision::BouncingVelocity, CollisionCount, and Collisions.

Referenced by ImpulseTransfers().

        {
            Collision* contact = 0;
            for ( unsigned i = 0; i < CollisionCount; i++ )
            {
                if ( Collisions[i].BouncingVelocity > eps )
                {
                    eps = Collisions[i].BouncingVelocity;
                    contact = &Collisions[i];
                }
            }
            return contact;
        }

Collision* WoRB::CollisionResolver::FindLargestPenetration ( double eps ) [inline]

Finds a collision with the largest penetration.

Definition at line 205 of file CollisionResolver.h.

References CollisionCount, Collisions, and WoRB::Collision::Penetration.

Referenced by PositionProjections().

        {
            Collision* contact = 0;
            for ( unsigned i = 0; i < CollisionCount; ++i )
            {
                if ( Collisions[i].Penetration > eps )
                {
                    eps = Collisions[i].Penetration;
                    contact = &Collisions[i];
                }
            }
            return contact;
        }

bool WoRB::CollisionResolver::HasSpaceForMoreContacts ( ) const [inline]

Checks if there is a free space available for more contact data.

Definition at line 99 of file CollisionResolver.h.

References FreeCount.

Referenced by WoRB::Sphere::Check(), WoRB::Cuboid::Check(), WoRB::Geometry::Detect(), and WoRB::Cuboid::RegisterContactOnAxis_Thorough().

        {
            return FreeCount > 0;
        }

void CollisionResolver::ImpulseTransfers	(	double	timeStep,
		unsigned	maxIterations = `0`,
		double	velocityEPS = `0.01`
	)

Resolves collisions using the impulse transfer method.

Definition at line 17 of file ImpulseMethod.cpp.

References WoRB::Collision::ActivateInactiveBodies(), WoRB::Collision::Body_A, WoRB::Collision::BouncingVelocity, CollisionCount, Collisions, WoRB::Quaternion::Cross(), FindLargestBouncingVelocity(), WoRB::Collision::GetBouncingVelocity(), WoRB::Collision::ImpulseTransfer(), WoRB::Collision::RelativePosition, WoRB::Collision::ToWorld, WoRB::QTensor::TransformInverse(), and WoRB::Collision::Velocity.

Referenced by WoRB::WorldOfRigidBodies< 256, 1024 >::SolveODE().

{
    if ( CollisionCount == 0 ) {
        return; // Nothing to do
    }

    // Setup default parameters
    //
    if ( maxIterations == 0 ) {
        maxIterations = 8 * CollisionCount;
    }
    if ( eps == 0 ) {
        eps = 0.01;
    }

    // Iterate performing impulse transfers, until there are no contacts with
    // notable bouncing velocity jolts are found.
    //
    for ( unsigned iteration = 0; iteration < maxIterations; ++iteration )
    {
        // Find the contact with the largest possible velocity jolt
        //
        Collision* contact = FindLargestBouncingVelocity( eps );
        if ( ! contact ) {
            break; // Done, if bouncing velocity are not found
        }

        // Activate bodies participating in the collision that are lying inactive
        //
        contact->ActivateInactiveBodies ();

        // Calculate velocity and angular velocity jolts
        //
        Quaternion V_jolt[2], W_jolt[2];
        contact->ImpulseTransfer( V_jolt, W_jolt );

        // With the change in velocity of the two bodies, the update of
        // contact velocities means that some of the relative closing
        // velocities need recomputing.
        //
        RigidBody** bodies_in_contact = &contact->Body_A;
        for( unsigned i = 0; i < CollisionCount; ++i )
        {
            Collision& c_i = Collisions[i];
            RigidBody** b_i = &c_i.Body_A;

            for( unsigned a = 0; a < 2; ++a ) // Each body in contact
            {
                if ( ! b_i[a] ) { 
                    continue; // Skip scenery objects
                }

                // Check for a match with each body in the newly resolved contact
                //
                for( unsigned b = 0; b < 2; ++b )
                {
                    if ( b_i[a] != bodies_in_contact[b] ) {
                        continue;
                    }

                    // dV = V_j + ( W_j x r )
                    //
                    Quaternion delta_V = 
                        V_jolt[b] + W_jolt[b].Cross( c_i.RelativePosition[a] );

                    // The sign of the change is negative if we're dealing
                    // with the second body in a contact.
                    //
                    Quaternion dV_world = c_i.ToWorld.TransformInverse( delta_V );
                    c_i.Velocity += a ? -dV_world : dV_world;

                    // Recalculate bouncing velocity (derived quantity).
                    // BouncingVelocity = - ( 1 + COR ) * Velocity.x
                    // where Velocity.x = < V_ab, Normal_ab >
                    //
                    c_i.BouncingVelocity = c_i.GetBouncingVelocity( h );
                }
            }
        }
    }
}

void WoRB::CollisionResolver::Initialize ( ) [inline]

Clears the collision registry.

Definition at line 116 of file CollisionResolver.h.

References CollisionCount, Collisions, FreeCount, MaxCollisionCount, and NextFree.

Referenced by WoRB::WorldOfRigidBodies< 256, 1024 >::InitializeODE(), and WoRB::WorldOfRigidBodies< 256, 1024 >::SolveODE().

        {
            NextFree = Collisions;
            FreeCount = MaxCollisionCount;
            CollisionCount = 0;
        }

const Collision& WoRB::CollisionResolver::operator[] ( unsigned index ) const [inline]

Gets collision data with the specified index.

Definition at line 106 of file CollisionResolver.h.

References Collisions.

        {
            return Collisions[ index ];
        }

void CollisionResolver::PositionProjections	(	unsigned	maxIterations = `0`,
		double	positionEPS = `0.01`
	)

Resolves collisions using the position projection method.

Definition at line 17 of file PositionProjections.cpp.

References WoRB::Collision::ActivateInactiveBodies(), WoRB::Collision::Body_A, CollisionCount, Collisions, WoRB::Quaternion::Cross(), WoRB::Quaternion::Dot(), FindLargestPenetration(), WoRB::Collision::Normal, WoRB::Collision::Penetration, WoRB::Collision::PositionProjection(), WoRB::Collision::RelativePosition, and Relaxation.

Referenced by WoRB::WorldOfRigidBodies< 256, 1024 >::SolveODE().

{
    if ( CollisionCount == 0 ) {
        return; // Nothing to do
    }

    // Setup default parameters
    //
    if ( maxIterations == 0 ) {
        maxIterations = 8 * CollisionCount;
    }
    if ( eps == 0 ) {
        eps = 1e-2;
    }

    // Performing position projections, until there are no contacts with notable 
    // penetrations found.
    //
    for ( unsigned iteration = 0; iteration < maxIterations; ++iteration )
    {
        // Find the contact with the largest penetration
        //
        Collision* contact = FindLargestPenetration( eps );
        if ( ! contact ) {
            break; // Done, if there are no penetrations
        }

        // Activate bodies participating in the collision that are lying inactive
        // @fixme (disabled since impulse transfer should wake bodies)
        contact->ActivateInactiveBodies ();

        // Calculate and apply position/orientation jolt that resolve the penetration
        // 
        Quaternion X_jolt[2], Q_jolt[2];
        contact->PositionProjection( X_jolt, Q_jolt, Relaxation );

        // However, the resolution may have changed the penetration of other
        // bodies, so we need to update affected collision data.
        //
        RigidBody** bodies_in_this_contact = &contact->Body_A;
        for ( unsigned i = 0; i < CollisionCount; ++i )
        {
            Collision& c_aff = Collisions[i];
            RigidBody** b_aff = &c_aff.Body_A;

            for ( unsigned a = 0; a < 2; ++a )  // For each body in scanned contacts
            {
                for ( unsigned b = 0; b < 2; ++b ) // For each body in this contact
                {
                    if ( b_aff[a] && b_aff[a] == bodies_in_this_contact[b] )
                    {
                        // dX = X_j + ( Q_j x R )
                        Quaternion deltaPosition = 
                            X_jolt[b] + Q_jolt[b].Cross( c_aff.RelativePosition[a] );

                        // The sign of the change is positive if we're dealing with 
                        // body B and negative otherwise, as the position resolution
                        // should be _subtracted_.
                        //
                        double dP_n = deltaPosition.Dot( c_aff.Normal );
                        c_aff.Penetration += a ? dP_n : -dP_n;
                    }
                }
            }
        }
    }
}

unsigned WoRB::CollisionResolver::RegisterNewContact	(	RigidBody *	body_A,
		RigidBody *	body_B,
		const Quaternion &	position,
		const Quaternion &	normal,
		double	penetration
	)		`[inline]`

Registers a new contact.

Returns:: 1 if ok, 0 if failed to allocate space for the contact.

Parameters:

body_A	The first body
body_B	The second body; 0 in case of scenery
position	The position of the contact in world frame
normal	The contact normal in world frame
penetration	The penetration depth

Definition at line 127 of file CollisionResolver.h.

References WoRB::Collision::Body_A, WoRB::Collision::Body_B, CollisionCount, FreeCount, WoRB::Collision::Friction, Friction, NextFree, WoRB::Collision::Normal, WoRB::Collision::Penetration, WoRB::Collision::Position, WoRB::Collision::Restitution, and Restitution.

Referenced by WoRB::Sphere::Check(), WoRB::Cuboid::Check(), WoRB::Cuboid::RegisterContactOnAxis(), and WoRB::Cuboid::RegisterContactOnAxis_Thorough().

        {
            if ( FreeCount <= 0 ) {
                return 0;
            }

            // Add contact the list of maintained collisions.
            //
            NextFree->Body_A        = body_A;
            NextFree->Body_B        = body_B;
            NextFree->Position      = position;
            NextFree->Normal        = normal;
            NextFree->Penetration   = penetration;
            NextFree->Friction      = Friction;
            NextFree->Restitution   = Restitution;

            // Reduce the number of remainging contacts and advance the array forward.
            //
            ++NextFree; --FreeCount;
            ++CollisionCount;

            return 1;
        }

void WoRB::CollisionResolver::UpdateDerivedQuantities ( double timeStep ) [inline]

Updates drived quantities (like contact velocity and axis info).

Definition at line 159 of file CollisionResolver.h.

References CollisionCount, Collisions, and WoRB::Collision::UpdateDerivedQuantities().

Referenced by WoRB::WorldOfRigidBodies< 256, 1024 >::SolveODE().

        {
            for ( unsigned i = 0; i < CollisionCount; ++i )
            {
                Collisions[i].UpdateDerivedQuantities( timeStep );
            }
        }