|
World of Rigid Bodies (WoRB)
|
|
World of Rigid Bodies (WoRB)
|
Encapsulates a cuboid (rectangular parallelepiped). More...
#include <Geometry.h>
Public Member Functions | |
| Cuboid () | |
| double | Volume () const |
| Gets the volume of the cuboid. | |
| void | SetMass (double mass) |
| Sets body mass and principal moment of inertia of the cuboid. | |
| bool | Intersects (const HalfSpace &plane) const |
| Tests for intersection of the cuboid and a half-space. | |
| bool | Intersects (const Cuboid &B) const |
| Tests for intersection between this and some other cuboid. | |
| unsigned | Check (CollisionResolver &owner, const HalfSpace &plane) const |
| Checks for collision between the cuboid and a half-space. | |
| unsigned | Check (CollisionResolver &owner, const Quaternion &point) const |
| Checks for collision between the cuboid and a point. | |
| unsigned | Check (CollisionResolver &owner, const Cuboid &B) const |
| Checks for collision between this and some other cuboid. | |
| unsigned | Check (CollisionResolver &owner, const Sphere &B) const |
| Checks for collision between the cuboid and a sphere. | |
Data Fields | |
| Quaternion | HalfExtent |
| Holds the half-extent of the cuboid along each of its local axes. | |
Private methods: Intersection detection related | |
| static double | Clamp (double x, double max) |
| Clamps the given value to some +/- max value. | |
| static Quaternion | FindContactPointOnEdges (const Quaternion &pt_A, const Quaternion &axis_A, double size_A, const Quaternion &pt_B, const Quaternion &axis_B, double size_B, bool use_A) |
| Finds point of contact between two points on two cuboid edges. | |
| double | ProjectOn (const Quaternion &vector) const |
| Gets the sum of cuboid's half-extent's projections on the given vector. | |
| double | GetPenetrationOnAxis (const Cuboid &B, const Quaternion &axis, const Quaternion &displacement) const |
| Checks if this and another cuboid overlap along the given direction. | |
| bool | IsOverlapOnAxis (const Cuboid &B, const Quaternion &direction, const Quaternion &displacement) const |
| Checks if this and another cuboid overlap along the given direction. | |
| bool | CheckOverlapOnAxis (const Cuboid &B, const Quaternion &direction, const Quaternion &displacement, double &smallestPenetration, unsigned tag_A, unsigned tag_B, unsigned &indexTag_A, unsigned &indexTag_B) const |
| Checks for overlap along the given direction. | |
| unsigned | RegisterContactOnAxis_Thorough (CollisionResolver &owner, const Cuboid &B, const Quaternion &displacement, unsigned axis) const |
| Registers a contact between this and the other cuboid body along an axis. | |
| unsigned | RegisterContactOnAxis (CollisionResolver &owner, const Cuboid &B, const Quaternion &displacement, const Quaternion &axis, double penetration) const |
| Registers a contact between this and the other cuboid body along an axis. | |
Encapsulates a cuboid (rectangular parallelepiped).
Definition at line 230 of file Geometry.h.
| WoRB::Cuboid::Cuboid | ( | ) | [inline] |
Definition at line 234 of file Geometry.h.
: Geometry( Geometry::_Cuboid ) { }
| unsigned Cuboid::Check | ( | CollisionResolver & | owner, |
| const HalfSpace & | plane | ||
| ) | const |
Checks for collision between the cuboid and a half-space.
Definition at line 557 of file CollisionDetection.cpp.
References WoRB::Geometry::Axis(), WoRB::Geometry::Body, WoRB::Quaternion::ComponentWiseProduct(), WoRB::HalfSpace::Direction, WoRB::Quaternion::Dot(), HalfExtent, WoRB::CollisionResolver::HasSpaceForMoreContacts(), Intersects(), WoRB::HalfSpace::Offset, WoRB::CollisionResolver::RegisterNewContact(), and WoRB::RigidBody::ToWorld.
{
if ( ! owner.HasSpaceForMoreContacts () ) {
return 0; // We do not have space left for new contacts
}
else if ( ! Intersects( plane ) ) {
return 0; // No intersection between the cuboid and the half-space
}
// The contact point with the largest penetration
// By default zero, meaning the mid-point on all edges of the cuboid.
//
Quaternion contactPoint;
// Find if there exist edges/faces of the cuboid parallel to the plane
Quaternion axis_n( 0,
Axis(0).Dot( plane.Direction ),
Axis(1).Dot( plane.Direction ),
Axis(2).Dot( plane.Direction )
);
unsigned parallelCount = 0;
for ( unsigned i = 0; i < 3; ++i ) // for each axis in B
{
// In case of cuboid's edge that is almost parallel to the plane
// we take a mid-point of the edge.
// Otherwise, if parallel, we take the vertex which is the closest.
//
if ( fabs( axis_n[i] ) < 1e-4 ) // almost parallel edge & plane
{
++parallelCount;
}
else // if cuboid's edge is not almost parallel to the plane
{
// Select the closest vertex
//
contactPoint[i] = axis_n[i] < 0 ? HalfExtent[i]
: -HalfExtent[i];
}
}
// In case if there are edges/faces parallel to the plane,
// just register the found mid-contact point and return.
//
if ( parallelCount > 0 )
{
// Switch to world coordinates and find out penetration of the
// contact point into the half-space
//
contactPoint = Body->ToWorld( contactPoint );
double penetration = plane.Offset - contactPoint.Dot( plane.Direction );
// Register a new contact at the point of contact
//
return owner.RegisterNewContact(
/* a */ Body,
/* b */ 0, // scenery
/* X */ contactPoint + 0.5 * penetration * plane.Direction,
/* N */ plane.Direction,
/* d */ penetration
);
}
// Proceede with the thorough investigation by scanning all vertices
// Find the intersection points by checking all vertices.
// If the cuboid is resting on a plane or on an edge, it will be reported
// as four or two contact points.
// Go through each combination of +/- for each half-size
//
unsigned contactCount = 0;
for ( unsigned i = 0; i < 8 && owner.HasSpaceForMoreContacts (); ++i )
{
// Calculate the position of each vertex
//
static const double vertices[8][3] =
{
{ 1, 1, 1 }, { -1, 1, 1 }, { 1, -1, 1 }, { -1, -1, 1 },
{ 1, 1, -1 }, { -1, 1, -1 }, { 1, -1, -1 }, { -1, -1, -1 }
};
Quaternion vertexPos( 0, vertices[i][0], vertices[i][1], vertices[i][2] );
vertexPos = vertexPos.ComponentWiseProduct( HalfExtent );
vertexPos = Body->ToWorld( vertexPos );
// Calculate the penetration of the vertex into the half-space
//
double penetration = plane.Offset - vertexPos.Dot( plane.Direction );
if ( penetration >= 0 ) // In case of penetration...
{
// Register a new contact where the point of contact is half-way between
// the vertex and the plane
//
contactCount += owner.RegisterNewContact(
/* a */ Body,
/* b */ 0, // scenery
/* X */ vertexPos + 0.5 * penetration * plane.Direction,
/* N */ plane.Direction,
/* d */ penetration
);
}
}
return contactCount;
}
| unsigned Cuboid::Check | ( | CollisionResolver & | owner, |
| const Quaternion & | point | ||
| ) | const |
Checks for collision between the cuboid and a point.
Definition at line 475 of file CollisionDetection.cpp.
References WoRB::Geometry::Axis(), WoRB::Geometry::Body, HalfExtent, WoRB::Const::Max, WoRB::CollisionResolver::RegisterNewContact(), WoRB::RigidBody::ToWorld, and WoRB::QTensor::TransformInverse().
{
// Transform the point into cuboid coordinates i.e. body-fixed frame
//
Quaternion pointInBodySpace = Body->ToWorld.TransformInverse( point );
Quaternion normal;
double min_depth = Const::Max;
// Check each axis, looking for the axis on which the penetration is least deep.
//
for ( unsigned i = 0; i < 3; ++i )
{
double depth = HalfExtent[i] - fabs( pointInBodySpace[i] );
if ( depth < 0 ) {
return 0;
}
else if ( depth < min_depth )
{
min_depth = depth;
normal = pointInBodySpace[i] < 0 ? -Axis(i) : Axis(i);
}
}
return owner.RegisterNewContact(
/* a */ Body,
/* b */ 0, // == scenery
/* X */ point,
/* N */ normal,
/* d */ min_depth
);
}
| unsigned Cuboid::Check | ( | CollisionResolver & | owner, |
| const Cuboid & | B | ||
| ) | const |
Checks for collision between this and some other cuboid.
Definition at line 351 of file CollisionDetection.cpp.
References WoRB::Geometry::Axis(), WoRB::Geometry::Body, WoRB::Quaternion::Cross(), WoRB::Quaternion::Dot(), FindContactPointOnEdges(), HalfExtent, WoRB::Const::Max, WoRB::Geometry::Position(), Quit_If_No_Overlaps, RegisterContactOnAxis(), WoRB::CollisionResolver::RegisterNewContact(), WoRB::RigidBody::ToWorld, and WoRB::Quaternion::Unit().
{
// Find the displacement between the two centra
//
Quaternion displacement = B.Position() - Position();
// Assume that there is no contact
//
double penetration = Const::Max;
unsigned int axisIndex_A = 0xFF;
unsigned int axisIndex_B = 0xFF;
#define Quit_If_No_Overlaps( axis, indexA, indexB ) \
if ( ! CheckOverlapOnAxis( B, (axis), displacement, penetration, \
(indexA), (indexB), axisIndex_A, axisIndex_B ) ) { \
return 0; \
}
// Check each body axes, keeping track of the axis with the smallest penetration
//
Quit_If_No_Overlaps( Axis(0) , 0, 0xFF );
Quit_If_No_Overlaps( Axis(1) , 1, 0xFF );
Quit_If_No_Overlaps( Axis(2) , 2, 0xFF );
Quit_If_No_Overlaps( B.Axis(0), 0xFF, 0 );
Quit_If_No_Overlaps( B.Axis(1), 0xFF, 1 );
Quit_If_No_Overlaps( B.Axis(2), 0xFF, 2 );
// Remember the single axis with the smallest penetration
//
bool use_A = axisIndex_B != 0xFF;
// Continue with the axes cross products
//
Quit_If_No_Overlaps( Axis(0).Cross( B.Axis(0) ), 0, 0 );
Quit_If_No_Overlaps( Axis(0).Cross( B.Axis(1) ), 0, 1 );
Quit_If_No_Overlaps( Axis(0).Cross( B.Axis(2) ), 0, 2 );
Quit_If_No_Overlaps( Axis(1).Cross( B.Axis(0) ), 1, 0 );
Quit_If_No_Overlaps( Axis(1).Cross( B.Axis(1) ), 1, 1 );
Quit_If_No_Overlaps( Axis(1).Cross( B.Axis(2) ), 1, 2 );
Quit_If_No_Overlaps( Axis(2).Cross( B.Axis(0) ), 2, 0 );
Quit_If_No_Overlaps( Axis(2).Cross( B.Axis(1) ), 2, 1 );
Quit_If_No_Overlaps( Axis(2).Cross( B.Axis(2) ), 2, 2 );
#undef Quit_If_No_Overlaps
// At this point we have detected a collision!
if ( axisIndex_B == 0xFF ) // single axis A
{
// The collision between cuboid B's vertex and this cuboid's face
//
return this->RegisterContactOnAxis( owner, B,
displacement, Axis( axisIndex_A ), penetration );
}
else if ( axisIndex_A == 0xFF ) // single axis B
{
// The collision between this cuboid's vertex and cuboid B's face
//
return B.RegisterContactOnAxis( owner, *this,
-displacement, B.Axis( axisIndex_B ), penetration );
}
// It is an edge-edge collision; get the involved axes from indices
//
Quaternion axis_A = Axis( axisIndex_A );
Quaternion axis_B = B.Axis( axisIndex_B );
Quaternion normal = axis_A.Cross( axis_B ).Unit ();
// Make the normal always point from A to B
//
if ( normal.Dot( displacement ) > 0 ) {
normal = -normal;
}
// Find the closest point on the involved edges to the other axes.
// By default the closest point is mid-point on the edge.
// @note Compare to RegisterContactOnAxis algorithm.
//
Quaternion ptOn_Edge_A; // = zero (mid-point on all edges by default)
Quaternion ptOn_Edge_B; // = zero (mid-point on all edges by default)
for ( unsigned i = 0; i < 3; ++i ) // foreach axis in A and B
{
if ( i != axisIndex_A )
{
double axis_An = Axis(i).Dot( normal );
if ( fabs( axis_An ) > 1e-4 ) {
ptOn_Edge_A[i] = axis_An > 0 ? -HalfExtent[i]
: HalfExtent[i];
}
}
if ( i != axisIndex_B )
{
double axis_Bn = B.Axis(i).Dot( normal );
if ( fabs( axis_Bn ) > 1e-4 ) {
ptOn_Edge_B[i] = axis_Bn > 0 ? B.HalfExtent[i]
: -B.HalfExtent[i];
}
}
}
// At this moment we have a point and a direction for the colliding edges.
// Now, find out the contact point i.e. the closest approach of B's line-segments.
// (Note that, from here, we continue in world coordinates.)
//
Quaternion contactPoint_world = FindContactPointOnEdges(
Body->ToWorld( ptOn_Edge_A ), axis_A, HalfExtent[ axisIndex_A ],
B.Body->ToWorld( ptOn_Edge_B ), axis_B, B.HalfExtent[ axisIndex_B ], use_A
);
return owner.RegisterNewContact(
/* a */ Body,
/* b */ B.Body,
/* X */ contactPoint_world,
/* N */ normal,
/* d */ penetration
);
}
| unsigned Cuboid::Check | ( | CollisionResolver & | owner, |
| const Sphere & | B | ||
| ) | const |
Checks for collision between the cuboid and a sphere.
Definition at line 510 of file CollisionDetection.cpp.
References WoRB::Geometry::Body, Clamp(), HalfExtent, WoRB::Geometry::Position(), WoRB::Sphere::Radius, WoRB::CollisionResolver::RegisterNewContact(), WoRB::RigidBody::ToWorld, WoRB::QTensor::TransformInverse(), WoRB::Quaternion::Unit(), WoRB::Quaternion::x, WoRB::Quaternion::y, and WoRB::Quaternion::z.
{
// Transform the center of the sphere into cuboid coordinates
//
Quaternion center = B.Position ();
Quaternion relCenter = Body->ToWorld.TransformInverse( center );
// Early out check to see if we can exclude the contact
//
if ( fabs( relCenter.x ) - B.Radius > HalfExtent.x ||
fabs( relCenter.y ) - B.Radius > HalfExtent.y ||
fabs( relCenter.z ) - B.Radius > HalfExtent.z )
{
return 0;
}
// Clamp each coordinate to the cuboid
//
Quaternion closestPoint( 0,
Clamp( relCenter.x, HalfExtent.x ),
Clamp( relCenter.y, HalfExtent.y ),
Clamp( relCenter.z, HalfExtent.z )
);
// Check we're in contact
//
double distance = ( closestPoint - relCenter ).ImSquaredNorm();
if ( distance > B.Radius * B.Radius ) {
return 0;
}
distance = sqrt( distance );
// New contact at the closest point in world coordinates
//
Quaternion closestPointWorld = Body->ToWorld( closestPoint );
return owner.RegisterNewContact(
/* a */ Body,
/* b */ B.Body,
/* X */ closestPointWorld,
/* N */ ( closestPointWorld - center ).Unit (),
/* d */ B.Radius - distance
);
}
| bool WoRB::Cuboid::CheckOverlapOnAxis | ( | const Cuboid & | B, |
| const Quaternion & | direction, | ||
| const Quaternion & | displacement, | ||
| double & | smallestPenetration, | ||
| unsigned | tag_A, | ||
| unsigned | tag_B, | ||
| unsigned & | indexTag_A, | ||
| unsigned & | indexTag_B | ||
| ) | const [inline, private] |
Checks for overlap along the given direction.
Keeps track of the smallest penetration.
Definition at line 386 of file Geometry.h.
References GetPenetrationOnAxis(), and WoRB::Quaternion::ImSquaredNorm().
{
// Skip almost parallel axes.
if ( direction.ImSquaredNorm() < 1e-4 ) {
return true;
}
// Get penetration depth on axis.
double penetration = GetPenetrationOnAxis( B, direction, displacement );
if ( penetration < 0 ) { // no penetration
return false;
}
else if ( penetration < smallestPenetration /*- 1e-6 */ ) {
smallestPenetration = penetration;
indexTag_A = tag_A;
indexTag_B = tag_B;
}
return true;
}
| static double WoRB::Cuboid::Clamp | ( | double | x, |
| double | max | ||
| ) | [inline, static, private] |
Clamps the given value to some +/- max value.
Definition at line 338 of file Geometry.h.
Referenced by Check().
{
return x > max ? max : x < -max ? -max : x;
}
| Quaternion Cuboid::FindContactPointOnEdges | ( | const Quaternion & | pt_A, |
| const Quaternion & | axis_A, | ||
| double | size_A, | ||
| const Quaternion & | pt_B, | ||
| const Quaternion & | axis_B, | ||
| double | size_B, | ||
| bool | use_A | ||
| ) | [inline, static, private] |
Finds point of contact between two points on two cuboid edges.
| pt_A | Point on the edge of the cuboid A |
| axis_A | Axis of the cuboid A |
| size_A | Half-extent of A's axis |
| pt_B | Point on the edge of the cuboid B |
| axis_B | Axis of the cuboid B |
| size_B | Half-extent of B's axis |
| use_A | Which cuboid to use, if point is on the edge |
Definition at line 180 of file CollisionDetection.cpp.
References WoRB::Quaternion::Dot(), and WoRB::Quaternion::ImSquaredNorm().
Referenced by Check().
{
// If use_A is true, and the contact point is outside the edge (in the case of
// an edge-face contact) then we use 'this' cuboid's midpoint, otherwise we use B's.
double sqNorm_dA = axis_A.ImSquaredNorm(); // d_A = Axis A
double sqNorm_dB = axis_B.ImSquaredNorm(); // d_B = Axis B
double axis_AB = axis_B.Dot( axis_A ); // Scalar product axis_A and axis_B
// Displacement between point on edge of this cuboid (A) and point on edge B
//
Quaternion p_AB = ptOn_A - ptOn_B;
double dpSta_A = p_AB.Dot( axis_A );
double dpSta_B = p_AB.Dot( axis_B );
double denominator = sqNorm_dA * sqNorm_dB - axis_AB * axis_AB;
// In case of parallel lines
//
if ( fabs( denominator ) < 1e-4 ) {
return use_A ? ptOn_A : ptOn_B;
}
double mu_A = ( axis_AB * dpSta_B - sqNorm_dB * dpSta_A ) / denominator;
double mu_B = ( sqNorm_dA * dpSta_B - axis_AB * dpSta_A ) / denominator;
// If either of the edges has the nearest point out of bounds,
// then the edges aren't crossed, we have an edge-face contact.
//
if ( mu_A > edge_A || mu_A < -edge_A || mu_B > edge_B || mu_B < -edge_B )
{
// Our point is on the edge of body which we know from the use_A parameter.
return use_A ? ptOn_A : ptOn_B;
}
else
{
return ( ptOn_A + axis_A * mu_A ) * 0.5
+ ( ptOn_B + axis_B * mu_B ) * 0.5;
}
}
| double WoRB::Cuboid::GetPenetrationOnAxis | ( | const Cuboid & | B, |
| const Quaternion & | axis, | ||
| const Quaternion & | displacement | ||
| ) | const [inline, private] |
Checks if this and another cuboid overlap along the given direction.
Definition at line 354 of file Geometry.h.
References WoRB::Quaternion::Dot(), ProjectOn(), and WoRB::Quaternion::Unit().
Referenced by CheckOverlapOnAxis(), and IsOverlapOnAxis().
{
Quaternion direction = axis.Unit ();
// Project the half-extents and displacement onto axis
double proj_A = ProjectOn( direction );
double proj_B = B.ProjectOn( direction );
double distance = fabs( displacement.Dot( direction ) );
// Return the overlap
return proj_A + proj_B - distance;
}
| bool WoRB::Cuboid::Intersects | ( | const HalfSpace & | plane | ) | const [inline] |
Tests for intersection of the cuboid and a half-space.
Definition at line 272 of file Geometry.h.
References WoRB::HalfSpace::Direction, WoRB::Quaternion::Dot(), WoRB::HalfSpace::Offset, WoRB::Geometry::Position(), and ProjectOn().
Referenced by Check().
{
// Calculate the projected radius of the cuboid onto the plane direction
double projectedRadius = ProjectOn( plane.Direction );
// Calculate how far the cuboid is from the origin
double distance = plane.Direction.Dot( Position() ) - projectedRadius;
// Check for the intersection
return distance <= plane.Offset;
}
| bool WoRB::Cuboid::Intersects | ( | const Cuboid & | B | ) | const [inline] |
Tests for intersection between this and some other cuboid.
Definition at line 286 of file Geometry.h.
References WoRB::Geometry::Axis(), WoRB::Quaternion::Cross(), IsOverlapOnAxis(), and WoRB::Geometry::Position().
{
// Find the displacement between the centra of two cuboids
//
Quaternion displacement = B.Position() - Position();
// Now, first, check axes of A, then axes of B, and finally their cross products
//
return IsOverlapOnAxis( B, Axis(0), displacement )
&& IsOverlapOnAxis( B, Axis(1), displacement )
&& IsOverlapOnAxis( B, Axis(2), displacement )
&& IsOverlapOnAxis( B, B.Axis(0), displacement )
&& IsOverlapOnAxis( B, B.Axis(1), displacement )
&& IsOverlapOnAxis( B, B.Axis(2), displacement )
&& IsOverlapOnAxis( B, Axis(0).Cross( B.Axis(0) ), displacement )
&& IsOverlapOnAxis( B, Axis(0).Cross( B.Axis(1) ), displacement )
&& IsOverlapOnAxis( B, Axis(0).Cross( B.Axis(2) ), displacement )
&& IsOverlapOnAxis( B, Axis(1).Cross( B.Axis(0) ), displacement )
&& IsOverlapOnAxis( B, Axis(1).Cross( B.Axis(1) ), displacement )
&& IsOverlapOnAxis( B, Axis(1).Cross( B.Axis(2) ), displacement )
&& IsOverlapOnAxis( B, Axis(2).Cross( B.Axis(0) ), displacement )
&& IsOverlapOnAxis( B, Axis(2).Cross( B.Axis(1) ), displacement )
&& IsOverlapOnAxis( B, Axis(2).Cross( B.Axis(2) ), displacement );
}
| bool WoRB::Cuboid::IsOverlapOnAxis | ( | const Cuboid & | B, |
| const Quaternion & | direction, | ||
| const Quaternion & | displacement | ||
| ) | const [inline, private] |
Checks if this and another cuboid overlap along the given direction.
Definition at line 371 of file Geometry.h.
References GetPenetrationOnAxis(), and WoRB::Quaternion::ImSquaredNorm().
Referenced by Intersects().
{
// Skip almost parallel axes.
if ( direction.ImSquaredNorm() < 1e-4 ) {
return true;
}
return GetPenetrationOnAxis( B, direction, displacement ) > 0;
}
| double WoRB::Cuboid::ProjectOn | ( | const Quaternion & | vector | ) | const [inline, private] |
Gets the sum of cuboid's half-extent's projections on the given vector.
Definition at line 344 of file Geometry.h.
References WoRB::Geometry::Axis(), WoRB::Quaternion::Dot(), HalfExtent, WoRB::Quaternion::x, WoRB::Quaternion::y, and WoRB::Quaternion::z.
Referenced by GetPenetrationOnAxis(), and Intersects().
{
return HalfExtent.x * fabs( vector.Dot( Axis(0) ) )
+ HalfExtent.y * fabs( vector.Dot( Axis(1) ) )
+ HalfExtent.z * fabs( vector.Dot( Axis(2) ) );
}
| unsigned Cuboid::RegisterContactOnAxis | ( | CollisionResolver & | owner, |
| const Cuboid & | B, | ||
| const Quaternion & | displacement, | ||
| const Quaternion & | axis, | ||
| double | penetration | ||
| ) | const [private] |
Registers a contact between this and the other cuboid body along an axis.
Faster variant that does not check every vertex but the closest one.
| owner | The collision registry |
| B | The second cuboid |
| displacement | The displacement between B and A centra |
| axis | The axis of this couboid (cuboid A) |
| penetration | The penetration depth |
Definition at line 283 of file CollisionDetection.cpp.
References WoRB::Geometry::Axis(), WoRB::Geometry::Body, WoRB::Quaternion::Dot(), HalfExtent, WoRB::CollisionResolver::RegisterNewContact(), and WoRB::RigidBody::ToWorld.
Referenced by Check().
{
// Find out which of the B faces is on the specified axis.
//
Quaternion normal = axis;
if ( normal.Dot( displacement ) > 0 ) {
normal = -normal;
}
Quaternion axis_Bn( 0,
B.Axis(0).Dot( normal ),
B.Axis(1).Dot( normal ),
B.Axis(2).Dot( normal )
);
// Find out which vertex of cuboid B we are colliding with.
Quaternion contactPointOn_B; // = zero
for ( unsigned i = 0; i < 3; ++i ) // for each axis in B
{
// In case of B's edge that is almost normal to the contact normal
// we take a mid-point of A's & B's edges intersection projected on B.Axis(i).
// Otherwise, if not normal, we take the vertex which is the closest.
//
if ( fabs( axis_Bn[i] ) < 1e-4 )
{
// Project the A's half-extents and displacement onto B's axis
// deltaCenter +/- halfExtent_A describes A's vertices in B's frame of ref.
//
double distance_BA = -displacement.Dot( B.Axis(i) );
double halfExtent_A = ProjectOn( B.Axis(i) );
double halfExtent_B = B.HalfExtent[i];
// Get mid-point of A's & B's projections' intersection on B's axis
//
double vxL = std::max( distance_BA - halfExtent_A, -halfExtent_B );
double vxR = std::min( distance_BA + halfExtent_A, halfExtent_B );
double vxM = 0.5 * ( vxL + vxR );
contactPointOn_B[i] = fabs( vxM ) < 1e-4 ? 0 : vxM;
}
else // if B's edge is not almost normal to the contact normal
{
// Select the closest vertex
//
contactPointOn_B[i] = axis_Bn[i] > 0 ? B.HalfExtent[i]
: -B.HalfExtent[i];
}
}
// No parallel axes with faces found; single collision point
//
return owner.RegisterNewContact(
/* a */ Body,
/* b */ B.Body,
/* X */ B.Body->ToWorld( contactPointOn_B ),
/* N */ normal,
/* d */ penetration
);
}
| unsigned Cuboid::RegisterContactOnAxis_Thorough | ( | CollisionResolver & | owner, |
| const Cuboid & | B, | ||
| const Quaternion & | displacement, | ||
| unsigned | axis | ||
| ) | const [private] |
Registers a contact between this and the other cuboid body along an axis.
| owner | The collision registry |
| B | The second cuboid |
| displacement | The displacement between B and A centra |
| axis | A's axis |
Definition at line 228 of file CollisionDetection.cpp.
References WoRB::Geometry::Body, WoRB::Quaternion::ComponentWiseProduct(), WoRB::Quaternion::Dot(), HalfExtent, WoRB::CollisionResolver::HasSpaceForMoreContacts(), WoRB::CollisionResolver::RegisterNewContact(), and WoRB::RigidBody::ToWorld.
{
Quaternion En = Axis( axis );
bool onRight = En.Dot( displacement ) > 0;
// Go through each combination of +/- for each half-size
//
unsigned contactCount = 0;
for ( unsigned i = 0; i < 8 && owner.HasSpaceForMoreContacts (); ++i )
{
// Calculate the position of each vertex
//
static const double vertices[8][3] =
{
{ 1, 1, 1 }, { -1, 1, 1 }, { 1, -1, 1 }, { -1, -1, 1 },
{ 1, 1, -1 }, { -1, 1, -1 }, { 1, -1, -1 }, { -1, -1, -1 }
};
Quaternion vertexPos( 0, vertices[i][0], vertices[i][1], vertices[i][2] );
vertexPos = vertexPos.ComponentWiseProduct( B.HalfExtent );
vertexPos = B.Body->ToWorld( vertexPos );
// Calculate the distance between the B's vertex and the A's center
// projected on A's axis
//
double distance = ( vertexPos - Position() ).Dot( En );
// Compare this to the A's face distance from A's center
//
if ( ( onRight && distance <= HalfExtent[ axis ] )
|| ( !onRight && distance >= -HalfExtent[ axis ] ) )
{
// New contact with the vertex as the point of contact
//
contactCount += owner.RegisterNewContact(
/* a */ Body,
/* b */ B.Body,
/* X */ vertexPos,
/* N */ onRight ? -En : En,
/* d */ onRight ? HalfExtent[ axis ] - distance
: distance - HalfExtent[ axis ]
);
}
}
return contactCount;
}
| void WoRB::Cuboid::SetMass | ( | double | mass | ) | [inline] |
Sets body mass and principal moment of inertia of the cuboid.
Definition at line 252 of file Geometry.h.
References WoRB::Geometry::Body, WoRB::RigidBody::CalculateDerivedQuantities(), WoRB::Quaternion::ComponentWiseProduct(), HalfExtent, WoRB::RigidBody::SetMomentOfInertia(), WoRB::RigidBody::SetupMass(), WoRB::Quaternion::x, WoRB::Quaternion::y, and WoRB::Quaternion::z.
Referenced by WoRB::Box::Box(), and WoRB_TestBed::ReconfigureTestBed().
{
Body->SetupMass( mass );
Quaternion extent = 2.0 * HalfExtent;
Quaternion sq = extent.ComponentWiseProduct( extent );
Body->SetMomentOfInertia( QTensor(
mass * ( sq.y + sq.z ) / 12,
mass * ( sq.x + sq.z ) / 12,
mass * ( sq.x + sq.y ) / 12
) );
Body->CalculateDerivedQuantities( /*fromMomenta*/ false );
}
| double WoRB::Cuboid::Volume | ( | ) | const [inline] |
Gets the volume of the cuboid.
Definition at line 245 of file Geometry.h.
References HalfExtent, WoRB::Quaternion::x, WoRB::Quaternion::y, and WoRB::Quaternion::z.
{
return 8.0 * HalfExtent.x * HalfExtent.y * HalfExtent.z;
}
Holds the half-extent of the cuboid along each of its local axes.
Definition at line 241 of file Geometry.h.
Referenced by WoRB::Box::Box(), Check(), WoRB_TestBed::Dump(), ProjectOn(), RegisterContactOnAxis(), RegisterContactOnAxis_Thorough(), WoRB::Box::Render(), WoRB::Box::RenderWireframe(), SetMass(), and Volume().
1.8.0