private static RenderedNode NewRenderedBox(int id, int index, CollisionOBB box)
{
RenderedNode n = UnscaledRenderedObject("unit_box.mesh", id, index, box.center);
n.node.ScaleFactor = 2 * box.extents * MO.DivMeter;
Quaternion q = Quaternion.Identity;
q.FromAxes(box.axes[0], box.axes[1], box.axes[2]);
n.node.Orientation = q;
return(n);
}
public override CollisionShape Clone()
{
Vector3[] newAxes = new Vector3[3];
newAxes[0] = axes[0];
newAxes[1] = axes[1];
newAxes[2] = axes[2];
CollisionOBB rv = new CollisionOBB(center, newAxes, extents);
rv.handle = this.handle;
rv.timeStamp = this.timeStamp;
return(rv);
}
public static void CaseNoZeros(ref Vector3 rkPnt, Vector3 rkDir,
CollisionOBB rkBox, SegOBBParams pf,
ref float rfSqrDistance)
{
Vector3 kPmE = new Vector3(rkPnt.x - rkBox.extents[0],
rkPnt.y - rkBox.extents[1],
rkPnt.z - rkBox.extents[2]);
float fProdDxPy, fProdDyPx, fProdDzPx, fProdDxPz, fProdDzPy, fProdDyPz;
fProdDxPy = rkDir.x*kPmE.y;
fProdDyPx = rkDir.y*kPmE.x;
if (fProdDyPx >= fProdDxPy) {
fProdDzPx = rkDir.z*kPmE.x;
fProdDxPz = rkDir.x*kPmE.z;
if (fProdDzPx >= fProdDxPz) {
// line intersects x = e0
Face(0,1,2,ref rkPnt,rkDir,rkBox,kPmE,pf,ref rfSqrDistance);
} else {
// line intersects z = e2
Face(2,0,1,ref rkPnt,rkDir,rkBox,kPmE,pf,ref rfSqrDistance);
}
} else {
fProdDzPy = rkDir.z*kPmE.y;
fProdDyPz = rkDir.y*kPmE.z;
if (fProdDzPy >= fProdDyPz)
{
// line intersects y = e1
Face(1,2,0,ref rkPnt,rkDir,rkBox,kPmE,pf,ref rfSqrDistance);
} else {
// line intersects z = e2
Face(2,0,1,ref rkPnt,rkDir,rkBox,kPmE,pf,ref rfSqrDistance);
}
}
}
public static void Case000(ref Vector3 rkPnt, CollisionOBB rkBox,
ref float rfSqrDistance)
{
float fDelta;
for (int i=0; i<3; i++) {
if (rkPnt[i] < -rkBox.extents[i]) {
fDelta = rkPnt[i] + rkBox.extents[i];
rfSqrDistance += fDelta*fDelta;
rkPnt[i] = -rkBox.extents[i];
}
else if (rkPnt[i] > rkBox.extents[i]) {
fDelta = rkPnt[i] - rkBox.extents[i];
rfSqrDistance += fDelta*fDelta;
rkPnt[i] = rkBox.extents[i];
}
}
}
public static bool TestOBBOBB(CollisionOBB a, CollisionOBB b, CollisionParms parms)
{
if (MO.DoLog)
MO.Log("TestOBBOBB Entering: a {0} b {1}", a, b);
if (TestOBBOBBInternal(a, b)) {
Vector3 n = b.center - a.center;
// ??? Not right
parms.SetNormPart(n);
parms.SetNormObstacle(-n);
if (MO.DoLog)
MO.Log("TestOBBOBB Collided: n {0}", n);
return true;
}
else
return false;
}
public static bool TestCapsuleOBB(CollisionCapsule a, CollisionOBB b, CollisionParms parms)
{
SegOBBParams obbParms = new SegOBBParams(true);
Segment s = Segment.SegmentFromStartAndEnd(a.bottomcenter, a.topcenter);
if (SqrDistSegOBB(s, b, obbParms) < a.capRadius * a.capRadius) {
// If parms.pfLParam == 0, closest is bottomcenter; if == 1,
// closest is topcenter.
Vector3 d = a.bottomcenter + obbParms.pfLParam * s.direction;
// pfBVec is relative to the center of the box, but in box
// coords.
Vector3 f = b.center;
for (int i=0; i<3; i++)
f += obbParms.pfBVec[i] * b.axes[i];
Vector3 n = f - d;
parms.SetNormPart(n);
parms.SetNormObstacle(-n);
if (MO.DoLog) {
MO.Log(" TestCapsuleOBB: pfLParam {0}, pfBVec {1}", obbParms.pfLParam, obbParms.pfBVec);
MO.Log(" TestCapsuleOBB: d {0}, f {1}", f, d);
MO.Log(" -n {0}", -n);
}
return true;
}
else
return false;
}
public static float SqrDistSegOBBInternal(Segment rkLine, CollisionOBB rkBox,
SegOBBParams pf)
{
// compute coordinates of line in box coordinate system
Vector3 kDiff = rkLine.origin - rkBox.center;
Vector3 kPnt = new Vector3(kDiff.Dot(rkBox.axes[0]),
kDiff.Dot(rkBox.axes[1]),
kDiff.Dot(rkBox.axes[2]));
Vector3 kDir = new Vector3(rkLine.direction.Dot(rkBox.axes[0]),
rkLine.direction.Dot(rkBox.axes[1]),
rkLine.direction.Dot(rkBox.axes[2]));
// Apply reflections so that direction vector has nonnegative components.
bool[] bReflect = new bool[3] { false, false, false };
int i;
for (i = 0; i < 3; i++) {
if (kDir[i] < 0.0f) {
kPnt[i] = -kPnt[i];
kDir[i] = -kDir[i];
bReflect[i] = true;
}
}
float fSqrDistance = 0.0f;
if (kDir.x > 0.0f) {
if (kDir.y > 0.0f) {
if (kDir.z > 0.0f) {
// (+,+,+)
CaseNoZeros(ref kPnt,kDir,rkBox,pf,ref fSqrDistance);
} else {
// (+,+,0)
Case0(0,1,2,ref kPnt,kDir,rkBox,pf,ref fSqrDistance);
}
} else {
if (kDir.z > 0.0f) {
// (+,0,+)
Case0(0,2,1,ref kPnt,kDir,rkBox,pf,ref fSqrDistance);
} else {
// (+,0,0)
Case00(0,1,2,ref kPnt,kDir,rkBox,pf,ref fSqrDistance);
}
}
} else {
if (kDir.y > 0.0f) {
if (kDir.z > 0.0f) {
// (0,+,+)
Case0(1,2,0,ref kPnt,kDir,rkBox,pf,ref fSqrDistance);
} else {
// (0,+,0)
Case00(1,0,2,ref kPnt,kDir,rkBox,pf,ref fSqrDistance);
}
} else {
if (kDir.z > 0.0f)
{
// (0,0,+)
Case00(2,0,1,ref kPnt,kDir,rkBox,pf,ref fSqrDistance);
} else {
// (0,0,0)
Case000(ref kPnt,rkBox,ref fSqrDistance);
if (pf.useIt)
pf.pfLParam = 0.0f;
}
}
}
if (pf.useIt) {
// undo reflections
for (i = 0; i < 3; i++) {
if (bReflect[i])
kPnt[i] = -kPnt[i];
}
pf.pfBVec = kPnt;
}
return fSqrDistance;
}
// Returns square of the distance from a point to an OBB
public static float SqDistPointOBB(Vector3 p, CollisionOBB b, out Vector3 c)
{
ClosestPtPointOBB(p, b, out c);
Vector3 v = p - c;
return v.Dot(v);
}
private static void TestPhysics( string srcDir, string dstDir, string name )
{
float OneMeter = 1000;
PhysicsData pd = new PhysicsData();
Vector3 center = new Vector3( 10 * OneMeter, 1 * OneMeter, 1 * OneMeter );
Vector3[] obbAxes = new Vector3[ 3 ];
obbAxes[ 0 ] = new Vector3( 1, 0, -1 );
obbAxes[ 0 ].Normalize();
obbAxes[ 1 ] = Vector3.UnitY;
obbAxes[ 2 ] = new Vector3( 1, 0, 1 );
obbAxes[ 2 ].Normalize();
Vector3 obbExtents = new Vector3( 2.5f * OneMeter, 1 * OneMeter, 1 * OneMeter );
CollisionOBB obb = new CollisionOBB( center, obbAxes, obbExtents );
pd.AddCollisionShape( "submesh01", obb );
CollisionAABB aabb = new CollisionAABB( center - obbExtents, center + obbExtents );
pd.AddCollisionShape( "submesh01", aabb );
CollisionSphere sphere = new CollisionSphere( center, 2 * OneMeter );
pd.AddCollisionShape( "submesh01", sphere );
Vector3 capExtents = new Vector3( -1 * OneMeter, 0, 0 );
CollisionCapsule capsule = new CollisionCapsule( center + capExtents, center - capExtents, .5f * OneMeter );
pd.AddCollisionShape( "submesh01", capsule );
PhysicsSerializer ps = new PhysicsSerializer();
ps.ExportPhysics( pd, name );
pd = new PhysicsData();
ps.ImportPhysics( pd, name );
foreach( string objectId in pd.GetCollisionObjects() )
{
log.InfoFormat( "Collision shapes for: {0}", objectId );
List<CollisionShape> collisionShapes = pd.GetCollisionShapes( objectId );
foreach( CollisionShape shape in collisionShapes )
log.InfoFormat( "Shape: {0}", shape );
}
}
// Keep the code below, commented out, because it is useful
// for debugging near plane issues
// private static int nearPlaneCollisionCount = 0;
// private static int nearPlaneNodeId = 99999;
// private static List<RenderedNode>[] nearPlaneRenderedNodes = new List<RenderedNode>[4] {
// new List<RenderedNode>(), new List<RenderedNode>(), new List<RenderedNode>(), new List<RenderedNode>() };
// private static CollisionCapsule[] nearPlaneCapsules = new CollisionCapsule[4];
//
// protected void CreateNearPlaneCapsules(Camera camera,
// Vector3 cameraPosition,
// Vector3 playerHeadPosition)
// {
// // Frame the clip plane with 4 narrow capsules
// // Start by finding the center of the near plane
// Vector3 toPlayer = (playerHeadPosition - cameraPosition).ToNormalized();
// Vector3 center = cameraPosition + (1.02f * camera.Near) * toPlayer;
// float thetaY = MathUtil.DegreesToRadians(camera.FOVy * 0.5f);
// float tanThetaY = MathUtil.Tan(thetaY);
// float tanThetaX = tanThetaY * camera.AspectRatio;
// Vector3 right = tanThetaX * camera.Near * (camera.Right.ToNormalized());
// Vector3 up = tanThetaY * camera.Near * (camera.Up.ToNormalized());
// Vector3 corner1 = center + right - up;
// Vector3 corner2 = center + right + up;
// Vector3 corner3 = center - right + up;
// Vector3 corner4 = center - right - up;
// nearPlaneCapsules[0] = new CollisionCapsule(corner1, corner2, Client.OneMeter * .001f);
// nearPlaneCapsules[1] = new CollisionCapsule(corner2, corner3, Client.OneMeter * .001f);
// nearPlaneCapsules[2] = new CollisionCapsule(corner3, corner4, Client.OneMeter * .001f);
// nearPlaneCapsules[3] = new CollisionCapsule(corner4, corner1, Client.OneMeter * .001f);
//
// RenderedNode.Scene = sceneManager;
// for (int i=0; i<4; i++) {
// RenderedNode.RemoveNodeRendering(nearPlaneNodeId + i, ref nearPlaneRenderedNodes[i]);
// RenderedNode.CreateRenderedNodes(nearPlaneCapsules[i], nearPlaneNodeId + i, ref nearPlaneRenderedNodes[i]);
// }
// }
protected float FindAcceptableCameraPosition(Camera camera, Vector3 cameraPos, Vector3 cameraTarget)
{
//Logger.Log(0, "FindAcceptableCameraPosition cameraPos {0}, cameraTarget {1}", cameraPos, cameraTarget);
// If there is no CollisionAPI object, punt
CollisionAPI api = client.WorldManager.CollisionHelper;
if (api == null)
return (cameraPos - cameraTarget).Length;
// Start by finding the point at which a narrow capsule
// from the camera to the player intersects. If it
// doesn't, then set the point to the cameraPos.
// Make a very narrow capsule between the camera and the player's head
CollisionCapsule cap = new CollisionCapsule(cameraPos,
cameraTarget,
Client.OneMeter * .001f); // 1 millimeter
Vector3 newCameraPos = cameraPos;
Vector3 unitTowardCamera = (newCameraPos - cameraTarget).ToNormalized();
// Test for a collision, setting intersection if there is one.
Vector3 intersection = Vector3.Zero;
if (api.FindClosestCollision(cap, cameraTarget, cameraPos, ref intersection) != null) {
// There is A collision - - set the new camera pos to
// be the point of intersection
newCameraPos = intersection - unitTowardCamera * camera.Near;
//Logger.Log(0, "FindAcceptableCameraPosition: Thin cap collides at {0}", newCameraPos);
}
// Move a near-plane OBB forward from the point of
// intersection until we find a place where the near plane
// doesn't intersect any collision volumes
Vector3 center = newCameraPos - camera.Near * unitTowardCamera;
float thetaY = MathUtil.DegreesToRadians(camera.FOVy * 0.5f);
float tanThetaY = MathUtil.Tan(thetaY);
float tanThetaX = tanThetaY * camera.AspectRatio;
// Compute the OBB axes and extents
Vector3[] axes = new Vector3[3];
Vector3 extents = Vector3.Zero;
// The axis perpendicular to the near plane
float obbDepth = 100f; // 100 mm thick
axes[0] = -unitTowardCamera;
axes[1] = -axes[0].Cross(Vector3.UnitY).ToNormalized();
axes[2] = -axes[1].Cross(axes[0]).ToNormalized();
extents[0] = obbDepth;
extents[1] = camera.Near * tanThetaX;
extents[2] = camera.Near * tanThetaY;
float len = (newCameraPos - cameraTarget).Length;
float startingLen = len;
CollisionOBB obb = new CollisionOBB(center, axes, extents);
CollisionParms parms = new CollisionParms();
while (len >= minThirdPersonDistance) {
obb.center = cameraTarget + unitTowardCamera * len;
bool collides = api.ShapeCollides(obb, parms);
//Logger.Log(0, "FindAcceptableCameraPosition len {0}, obb center {1}, collides {2}", len, obb.center, collides);
if (!collides)
break;
//client.Write(string.Format("OBB collides; len {0}", len));
len -= obbDepth;
}
// Len is less than the camera min distance, so just
// return it
//Logger.Log(0, "FindAcceptableCameraPosition: starting len {0} final len {1}", startingLen, len);
return len;
}
public override CollisionShape Clone()
{
Vector3[] newAxes = new Vector3[3];
newAxes[0] = axes[0];
newAxes[1] = axes[1];
newAxes[2] = axes[2];
CollisionOBB rv = new CollisionOBB(center, newAxes, extents);
rv.handle = this.handle;
rv.timeStamp = this.timeStamp;
return rv;
}
public void ReadBox(string objectId, Matrix4 transform, XmlNode node, PhysicsData physicsData)
{
Quaternion rot = MathHelpers.GetRotation(transform);
Matrix4 tmpTransform = rot.Inverse().ToRotationMatrix() * transform;
Vector3 scaleDelta = tmpTransform.Scale - Vector3.UnitScale;
if (scaleDelta.Length > PhysicsSerializer.ScaleEpsilon)
log.Error("Scale is not currently supported for box shapes");
Vector3 halfExtents = Vector3.Zero;
foreach (XmlNode childNode in node.ChildNodes) {
switch (childNode.Name) {
case "half_extents":
ReadVector(ref halfExtents, childNode);
break;
default:
DebugMessage(childNode);
break;
}
}
Vector3 center = unitConversion * transform.Translation;
halfExtents *= unitConversion;
CollisionShape collisionShape;
float angle = 0;
Vector3 axis = Vector3.Zero;
rot.ToAngleAxis(ref angle, ref axis);
// I could build AABB shapes for boxes that are aligned, but then
// I can't drop instances in the world with arbitrary rotations.
// Instead, just always use an OBB.
//if (angle < PhysicsSerializer.RotateEpsilon) {
// collisionShape = new CollisionAABB(center - halfExtents, center + halfExtents);
//} else {
// TODO: I'm not sure I understand the OBB constructor
Vector3[] axes = new Vector3[3];
axes[0] = rot.XAxis;
axes[1] = rot.YAxis;
axes[2] = rot.ZAxis;
collisionShape = new CollisionOBB(center, axes, halfExtents);
physicsData.AddCollisionShape(objectId, collisionShape);
}
private static CollisionShape ExtractBox_Old( SubMesh subMesh )
{
if( DoLog )
{
Log( "" );
Log( string.Format( "Extracting box for submesh {0}", subMesh.Name ) );
}
MeshTriangle[] triangles = ExtractSubmeshTriangles( subMesh );
int count = triangles.Length;
Plane[] planesUnsorted = new Plane[ 6 ];
int planeCount = 0;
// Iterate through the triangles. For each triangle,
// determine the plane it belongs to, and find the plane
// in the array of planes, or if it's not found, add it.
for( int i = 0; i < count; i++ )
{
MeshTriangle t = triangles[ i ];
bool found = false;
Plane p = new Plane( t.vertPos[ 0 ], t.vertPos[ 1 ], t.vertPos[ 2 ] );
NormalizePlane( ref p );
if( DoLog )
Log( string.Format( " {0} => plane {1}", t.ToString(), p.ToString() ) );
for( int j = 0; j < planeCount; j++ )
{
Plane pj = planesUnsorted[ j ];
if( SamePlane( pj, p ) )
{
if( DoLog )
Log( string.Format( " plane {0} same as plane {1}", p.ToString(), pj.ToString() ) );
found = true;
break;
}
}
if( !found )
{
if( planeCount < 6 )
{
if( DoLog )
Log( string.Format( " plane[{0}] = plane {1}", planeCount, p.ToString() ) );
planesUnsorted[ planeCount++ ] = p;
}
else
Debug.Assert( false,
string.Format( "In the definition of box {0}, more than 6 planes were found",
subMesh.Name ) );
}
}
Debug.Assert( planeCount == 6,
string.Format( "In the definition of box {0}, fewer than 6 planes were found",
subMesh.Name ) );
// Now recreate the planes array, making sure that
// opposite faces are 3 planes apart
Plane[] planes = new Plane[ 6 ];
bool[] planeUsed = new bool[ 6 ];
for( int i = 0; i < 6; i++ )
planeUsed[ i ] = false;
int planePair = 0;
for( int i = 0; i < 6; i++ )
{
if( !planeUsed[ i ] )
{
Plane p1 = planesUnsorted[ i ];
planes[ planePair ] = p1;
planeUsed[ i ] = true;
for( int j = 0; j < 6; j++ )
{
Plane p2 = planesUnsorted[ j ];
if( !planeUsed[ j ] && !Orthogonal( p2, p1 ) )
{
planes[ 3 + planePair++ ] = p2;
planeUsed[ j ] = true;
break;
}
}
}
}
Debug.Assert( planePair == 3, "Didn't find 3 pairs of parallel planes" );
// Make sure that the sequence of planes follows the
// right-hand rule
if( planes[ 0 ].Normal.Cross( planes[ 1 ].Normal ).Dot( planes[ 3 ].Normal ) < 0 )
{
// Swap the first two plane pairs
Plane p = planes[ 0 ];
planes[ 0 ] = planes[ 1 ];
planes[ 1 ] = p;
p = planes[ 0 + 3 ];
planes[ 0 + 3 ] = planes[ 1 + 3 ];
planes[ 1 + 3 ] = p;
Debug.Assert( planes[ 0 ].Normal.Cross( planes[ 1 ].Normal ).Dot( planes[ 3 ].Normal ) > 0,
"Even after swapping, planes don't obey the right-hand rule" );
}
// Now we have our 6 planes, sorted so that opposite
// planes are 3 planes apart, and so they obey the
// right-hand rule. This guarantees that corners
// correspond. Find the 8 intersections that define the
// corners.
Vector3[] corners = new Vector3[ 8 ];
int cornerCount = 0;
for( int i = 0; i <= 3; i += 3 )
{
Plane p1 = planes[ i ];
for( int j = 1; j <= 4; j += 3 )
{
Plane p2 = planes[ j ];
for( int k = 2; k <= 5; k += 3 )
{
Plane p3 = planes[ k ];
Vector3 corner = -1 * ((p1.D * (p2.Normal.Cross( p3.Normal )) +
p2.D * (p3.Normal.Cross( p1.Normal )) +
p3.D * (p1.Normal.Cross( p2.Normal ))) /
p1.Normal.Dot( p2.Normal.Cross( p3.Normal ) ));
Debug.Assert( cornerCount < 8,
string.Format( "In the definition of box {0}, more than 8 corners were found",
subMesh.Name ) );
if( DoLog )
Log( string.Format( " corner#{0}: {1}", cornerCount, corner.ToString() ) );
corners[ cornerCount++ ] = corner;
}
}
}
Debug.Assert( cornerCount == 8,
string.Format( "In the definition of box {0}, fewer than 8 corners were found",
subMesh.Name ) );
// We know that corners correspond. Now find the center
Vector3 center = (corners[ 0 ] + corners[ 7 ]) / 2;
Debug.Assert( (center - (corners[ 1 ] + corners[ 5 ]) / 2.0f).Length > geometryEpsilon ||
(center - (corners[ 2 ] + corners[ 6 ]) / 2.0f).Length > geometryEpsilon ||
(center - (corners[ 3 ] + corners[ 7 ]) / 2.0f).Length > geometryEpsilon,
string.Format( "In the definition of box {0}, center definition {0} is not consistent",
subMesh.Name, center.ToString() ) );
// Find the extents
Vector3 extents = new Vector3( Math.Abs( (corners[ 1 ] - corners[ 0 ]).Length / 2.0f ),
Math.Abs( (corners[ 3 ] - corners[ 1 ]).Length / 2.0f ),
Math.Abs( (corners[ 4 ] - corners[ 0 ]).Length / 2.0f ) );
if( DoLog )
Log( string.Format( " extents {0}", extents.ToString() ) );
// Find the axes
Vector3[] axes = new Vector3[ 3 ] { (corners[1] - corners[0]).ToNormalized(),
(corners[3] - corners[1]).ToNormalized(),
(corners[4] - corners[0]).ToNormalized() };
if( DoLog )
{
for( int i = 0; i < 3; i++ )
{
Log( string.Format( " axis[{0}] {1}", i, axes[ i ] ) );
}
}
// Now, is it an obb or an aabb? Figure out if the axes
// point in the same direction as the basis vectors, and
// if so, the order of the axes
int[] mapping = new int[ 3 ] { -1, -1, -1 };
int foundMapping = 0;
for( int i = 0; i < 3; i++ )
{
for( int j = 0; j < 3; j++ )
{
if( axes[ i ].Cross( Primitives.UnitBasisVectors[ j ] ).Length < geometryEpsilon )
{
mapping[ i ] = j;
foundMapping++;
break;
}
}
}
CollisionShape shape;
if( foundMapping == 3 )
{
// It's an AABB, so build the min and max vectors, in
// the order that matches the unit basis vector order
Vector3 min = Vector3.Zero;
Vector3 max = Vector3.Zero;
for( int i = 0; i < 3; i++ )
{
float e = extents[ i ];
int j = mapping[ i ];
min[ j ] = center[ j ] - e;
max[ j ] = center[ j ] + e;
}
shape = new CollisionAABB( min, max );
}
else
{
Vector3 ruleTest = axes[ 0 ].Cross( axes[ 1 ] );
if( axes[ 2 ].Dot( ruleTest ) < 0 )
axes[ 2 ] = -1 * axes[ 2 ];
// Return the OBB
shape = new CollisionOBB( center, axes, extents );
}
if( DoLog )
Log( string.Format( "Extraction result: {0}", shape ) );
return shape;
}
// Take a different approach, based on an idea of Robin's:
// Find the farthest point pair, and use that to identify the
// triangles with one of the points as a vertex. Then take
// the normals of the triangles, adjust to object the
// right-hand rule, and they are the axes. Compute the center
// from the average of the farthest points, and extents by
// dotting farthest - center with the axes.
private static CollisionShape ExtractBox( SubMesh subMesh )
{
if( DoLog )
{
Log( "" );
Log( string.Format( "Extracting box for submesh {0}", subMesh.Name ) );
}
MeshTriangle[] triangles = ExtractSubmeshTriangles( subMesh );
int count = triangles.Length;
// Find the two farthest vertices across all triangle
Vector3[] farthestPoints = new Vector3[ 2 ] { Vector3.Zero, Vector3.Zero };
float farthestDistanceSquared = 0.0f;
for( int i = 0; i < count; i++ )
{
MeshTriangle t1 = triangles[ i ];
for( int j = 0; j < 3; j++ )
{
Vector3 p1 = t1.vertPos[ j ];
for( int r = i; r < count; r++ )
{
MeshTriangle t2 = triangles[ r ];
for( int s = 0; s < 3; s++ )
{
Vector3 p2 = t2.vertPos[ s ];
Vector3 diff = (p1 - p2);
float d = diff.LengthSquared;
// if (DoLog)
// Log(string.Format(" TriVert {0} {1} {2} / {3} {4} {5} dist {6}", i, j, p1, r, s, p2, d));
if( d > farthestDistanceSquared )
{
if( DoLog )
Log( string.Format( " Largest! TriVert {0} {1} {2} / {3} {4} {5} dist {6}",
i, j, p1, r, s, p2, d ) );
farthestDistanceSquared = d;
farthestPoints[ 0 ] = p1;
farthestPoints[ 1 ] = p2;
}
}
}
}
}
// The center is the average of the farthest points
Vector3 center = (farthestPoints[ 0 ] + farthestPoints[ 1 ]) * 0.5f;
if( DoLog )
{
Log( string.Format( "The farthest points are {0} and {1}",
farthestPoints[ 0 ], farthestPoints[ 1 ] ) );
Log( string.Format( "The center is {0}", center ) );
}
// Now find the three triangles that have the
// farthestPoints[0] as a vertex
Vector3[] axes = new Vector3[] { Vector3.Zero, Vector3.Zero, Vector3.Zero };
int foundCount = 0;
for( int i = 0; i < count; i++ )
{
MeshTriangle t = triangles[ i ];
for( int j = 0; j < 3; j++ )
{
Vector3 p = t.vertPos[ j ];
if( (p - farthestPoints[ 0 ]).LengthSquared < geometryEpsilon )
{
Vector3 side1 = t.vertPos[ 1 ] - t.vertPos[ 0 ];
Vector3 side2 = t.vertPos[ 2 ] - t.vertPos[ 1 ];
Vector3 axis = side1.Cross( side2 ).ToNormalized();
// Ignore this triangle if his normal matches one we already have
bool ignore = false;
for( int k = 0; k < foundCount; k++ )
{
if( Math.Abs( axis.Cross( axes[ k ] ).LengthSquared ) < geometryEpsilon )
{
ignore = true;
break;
}
}
if( !ignore )
{
Debug.Assert( foundCount < 3, "Found more than three triangles with distinct normals and vertex = farthest point" );
axes[ foundCount ] = axis;
foundCount++;
}
}
}
}
// Put the axes in coordinate order
for( int i = 0; i < 3; i++ )
{
float largest = float.MinValue;
int largestIndex = i;
for( int j = 0; j < 3; j++ )
{
float v = Math.Abs( axes[ j ][ i ] );
if( v > largest )
{
largestIndex = j;
largest = v;
}
}
if( largestIndex != i )
{
Vector3 t = axes[ i ];
axes[ i ] = axes[ largestIndex ];
axes[ largestIndex ] = t;
}
if( axes[ i ][ i ] < 0 )
axes[ i ] = -axes[ i ];
}
// Put the axes in right-hand-rule order
if( axes[ 0 ].Cross( axes[ 1 ] ).Dot( axes[ 2 ] ) < 0 )
{
axes[ 2 ] = -axes[ 2 ];
}
Debug.Assert( axes[ 0 ].Cross( axes[ 1 ] ).Dot( axes[ 2 ] ) > 0,
"After swapping axes, still don't obey right-hand rule" );
// The extents are just the abs of the dot products of
// farthest point minus the center with the axes
Vector3 f = farthestPoints[ 0 ] - center;
Vector3 extents = new Vector3( Math.Abs( f.Dot( axes[ 0 ] ) ),
Math.Abs( f.Dot( axes[ 1 ] ) ),
Math.Abs( f.Dot( axes[ 2 ] ) ) );
if( DoLog )
{
for( int i = 0; i < 3; i++ )
{
Log( string.Format( " axis[{0}] {1}, extent[{2}] {3}", i, axes[ i ], i, extents[ i ] ) );
}
int[] sign = new int[ 3 ] { 0, 0, 0 };
for( int i = -1; i < 2; i += 2 )
{
sign[ 0 ] = i;
for( int j = -1; j < 2; j += 2 )
{
sign[ 1 ] = j;
for( int k = -1; k < 2; k += 2 )
{
sign[ 2 ] = k;
Vector3 corner = center;
for( int a = 0; a < 3; a++ )
corner += axes[ a ] * extents[ a ] * sign[ a ];
Log( string.Format( " corner[{0},{1},{2}] = {3}", i, j, k, corner ) );
}
}
}
}
// Now, is it an obb or an aabb? Figure out if the axes
// point in the same direction as the basis vectors, and
// if so, the order of the axes
int[] mapping = new int[ 3 ] { -1, -1, -1 };
int foundMapping = 0;
for( int i = 0; i < 3; i++ )
{
for( int j = 0; j < 3; j++ )
{
if( Math.Abs( axes[ i ].Dot( Primitives.UnitBasisVectors[ j ] ) - 1.0f ) < .0001f )
{
if( DoLog )
Log( string.Format( " foundMapping[{0}], basis vector {1}", i, Primitives.UnitBasisVectors[ j ] ) );
mapping[ i ] = j;
foundMapping++;
break;
}
}
}
CollisionShape shape;
if( foundMapping == 3 )
{
// It's an AABB, so build the min and max vectors, in
// the order that matches the unit basis vector order
Vector3 min = Vector3.Zero;
Vector3 max = Vector3.Zero;
for( int i = 0; i < 3; i++ )
{
float e = extents[ i ];
int j = mapping[ i ];
min[ j ] = center[ j ] - e;
max[ j ] = center[ j ] + e;
}
shape = new CollisionAABB( min, max );
}
else
// Return the OBB
shape = new CollisionOBB( center, axes, extents );
if( DoLog )
Log( string.Format( "Extraction result: {0}", shape ) );
return shape;
}
// Given point p, return point q on the surface of OBB b, closest to p
public static void ClosestPtPointOBB(Vector3 p, CollisionOBB b, out Vector3 q)
{
Vector3 d = p - b.center;
// Make sure P is on the surface of the OBB
float pSquared = d.Dot(d);
float bSquared = b.radius * b.radius;
if (pSquared < bSquared) {
p = p * (float)Math.Sqrt(bSquared / pSquared);
}
// Start result at center of box; make steps from there
q = b.center;
// For each OBB axis...
for (int i = 0; i < 3; i++) {
// ...project d onto that axis to get the distance
// along the axis of d from the box center
float dist = d.Dot(b.axes[i]);
// If distance farther than the box extents, clamp to the box
float e = b.extents[i];
dist = Clamp(dist, -e, e);
// Step that distance along the axis to get world coordinate
q += dist * b.axes[i];
}
}
public static void Face(int i0, int i1, int i2, ref Vector3 rkPnt,
Vector3 rkDir, CollisionOBB rkBox,
Vector3 rkPmE, SegOBBParams pf,
ref float rfSqrDistance)
{
Vector3 kPpE = Vector3.Zero;
float fLSqr, fInv, fTmp, fParam, fT, fDelta;
kPpE[i1] = rkPnt[i1] + rkBox.extents[i1];
kPpE[i2] = rkPnt[i2] + rkBox.extents[i2];
if (rkDir[i0]*kPpE[i1] >= rkDir[i1]*rkPmE[i0]) {
if (rkDir[i0]*kPpE[i2] >= rkDir[i2]*rkPmE[i0]) {
// v[i1] >= -e[i1], v[i2] >= -e[i2] (distance = 0)
if (pf.useIt) {
rkPnt[i0] = rkBox.extents[i0];
fInv = 1.0f/rkDir[i0];
rkPnt[i1] -= rkDir[i1]*rkPmE[i0]*fInv;
rkPnt[i2] -= rkDir[i2]*rkPmE[i0]*fInv;
pf.pfLParam = -rkPmE[i0]*fInv;
}
} else {
// v[i1] >= -e[i1], v[i2] < -e[i2]
fLSqr = rkDir[i0]*rkDir[i0] + rkDir[i2]*rkDir[i2];
fTmp = fLSqr*kPpE[i1] - rkDir[i1]*(rkDir[i0]*rkPmE[i0] +
rkDir[i2]*kPpE[i2]);
if (fTmp <= 2.0*fLSqr*rkBox.extents[i1]) {
fT = fTmp/fLSqr;
fLSqr += rkDir[i1]*rkDir[i1];
fTmp = kPpE[i1] - fT;
fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*fTmp +
rkDir[i2]*kPpE[i2];
fParam = -fDelta/fLSqr;
rfSqrDistance += rkPmE[i0]*rkPmE[i0] + fTmp*fTmp +
kPpE[i2]*kPpE[i2] + fDelta*fParam;
if (pf.useIt) {
pf.pfLParam = fParam;
rkPnt[i0] = rkBox.extents[i0];
rkPnt[i1] = fT - rkBox.extents[i1];
rkPnt[i2] = -rkBox.extents[i2];
}
} else {
fLSqr += rkDir[i1]*rkDir[i1];
fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*rkPmE[i1] +
rkDir[i2]*kPpE[i2];
fParam = -fDelta/fLSqr;
rfSqrDistance += rkPmE[i0]*rkPmE[i0] + rkPmE[i1]*rkPmE[i1] +
kPpE[i2]*kPpE[i2] + fDelta*fParam;
if (pf.useIt) {
pf.pfLParam = fParam;
rkPnt[i0] = rkBox.extents[i0];
rkPnt[i1] = rkBox.extents[i1];
rkPnt[i2] = -rkBox.extents[i2];
}
}
}
} else {
if (rkDir[i0]*kPpE[i2] >= rkDir[i2]*rkPmE[i0]) {
// v[i1] < -e[i1], v[i2] >= -e[i2]
fLSqr = rkDir[i0]*rkDir[i0] + rkDir[i1]*rkDir[i1];
fTmp = fLSqr*kPpE[i2] - rkDir[i2]*(rkDir[i0]*rkPmE[i0] +
rkDir[i1]*kPpE[i1]);
if (fTmp <= 2.0*fLSqr*rkBox.extents[i2]) {
fT = fTmp/fLSqr;
fLSqr += rkDir[i2]*rkDir[i2];
fTmp = kPpE[i2] - fT;
fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] +
rkDir[i2]*fTmp;
fParam = -fDelta/fLSqr;
rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] +
fTmp*fTmp + fDelta*fParam;
if (pf.useIt) {
pf.pfLParam = fParam;
rkPnt[i0] = rkBox.extents[i0];
rkPnt[i1] = -rkBox.extents[i1];
rkPnt[i2] = fT - rkBox.extents[i2];
}
} else {
fLSqr += rkDir[i2]*rkDir[i2];
fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] +
rkDir[i2]*rkPmE[i2];
fParam = -fDelta/fLSqr;
rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] +
rkPmE[i2]*rkPmE[i2] + fDelta*fParam;
if (pf.useIt) {
pf.pfLParam = fParam;
rkPnt[i0] = rkBox.extents[i0];
rkPnt[i1] = -rkBox.extents[i1];
rkPnt[i2] = rkBox.extents[i2];
}
}
} else {
// v[i1] < -e[i1], v[i2] < -e[i2]
fLSqr = rkDir[i0]*rkDir[i0]+rkDir[i2]*rkDir[i2];
fTmp = fLSqr*kPpE[i1] - rkDir[i1]*(rkDir[i0]*rkPmE[i0] +
rkDir[i2]*kPpE[i2]);
if (fTmp >= 0.0f) {
// v[i1]-edge is closest
if (fTmp <= 2.0*fLSqr*rkBox.extents[i1])
{
fT = fTmp/fLSqr;
fLSqr += rkDir[i1]*rkDir[i1];
fTmp = kPpE[i1] - fT;
fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*fTmp +
rkDir[i2]*kPpE[i2];
fParam = -fDelta/fLSqr;
rfSqrDistance += rkPmE[i0]*rkPmE[i0] + fTmp*fTmp +
kPpE[i2]*kPpE[i2] + fDelta*fParam;
if (pf.useIt)
{
pf.pfLParam = fParam;
rkPnt[i0] = rkBox.extents[i0];
rkPnt[i1] = fT - rkBox.extents[i1];
rkPnt[i2] = -rkBox.extents[i2];
}
} else {
fLSqr += rkDir[i1]*rkDir[i1];
fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*rkPmE[i1] +
rkDir[i2]*kPpE[i2];
fParam = -fDelta/fLSqr;
rfSqrDistance += rkPmE[i0]*rkPmE[i0] + rkPmE[i1]*rkPmE[i1]
+ kPpE[i2]*kPpE[i2] + fDelta*fParam;
if (pf.useIt) {
pf.pfLParam = fParam;
rkPnt[i0] = rkBox.extents[i0];
rkPnt[i1] = rkBox.extents[i1];
rkPnt[i2] = -rkBox.extents[i2];
}
}
return;
}
fLSqr = rkDir[i0]*rkDir[i0] + rkDir[i1]*rkDir[i1];
fTmp = fLSqr*kPpE[i2] - rkDir[i2]*(rkDir[i0]*rkPmE[i0] +
rkDir[i1]*kPpE[i1]);
if (fTmp >= 0.0f) {
// v[i2]-edge is closest
if (fTmp <= 2.0*fLSqr*rkBox.extents[i2]) {
fT = fTmp/fLSqr;
fLSqr += rkDir[i2]*rkDir[i2];
fTmp = kPpE[i2] - fT;
fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] +
rkDir[i2]*fTmp;
fParam = -fDelta/fLSqr;
rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] +
fTmp*fTmp + fDelta*fParam;
if (pf.useIt) {
pf.pfLParam = fParam;
rkPnt[i0] = rkBox.extents[i0];
rkPnt[i1] = -rkBox.extents[i1];
rkPnt[i2] = fT - rkBox.extents[i2];
}
} else {
fLSqr += rkDir[i2]*rkDir[i2];
fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] +
rkDir[i2]*rkPmE[i2];
fParam = -fDelta/fLSqr;
rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] +
rkPmE[i2]*rkPmE[i2] + fDelta*fParam;
if (pf.useIt) {
pf.pfLParam = fParam;
rkPnt[i0] = rkBox.extents[i0];
rkPnt[i1] = -rkBox.extents[i1];
rkPnt[i2] = rkBox.extents[i2];
}
}
return;
}
// (v[i1],v[i2])-corner is closest
fLSqr += rkDir[i2]*rkDir[i2];
fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] +
rkDir[i2]*kPpE[i2];
fParam = -fDelta/fLSqr;
rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] +
kPpE[i2]*kPpE[i2] + fDelta*fParam;
if (pf.useIt) {
pf.pfLParam = fParam;
rkPnt[i0] = rkBox.extents[i0];
rkPnt[i1] = -rkBox.extents[i1];
rkPnt[i2] = -rkBox.extents[i2];
}
}
}
}
private void FourCollisionsButton_Click(object sender, EventArgs e)
{
CollisionAPI API = new CollisionAPI();
// Create some obstacles, at 4 corners of a square
List<CollisionShape> shapes = new List<CollisionShape>();
CollisionSphere sphere = new CollisionSphere(new Vector3(0f, 0f, 2f), 1f);
shapes.Add(sphere);
API.AddCollisionShape(sphere, 1);
CollisionCapsule capsule = new CollisionCapsule(new Vector3(10f,0f,0f),
new Vector3(10f,0f,4f),
1f);
shapes.Add(capsule);
API.AddCollisionShape(capsule, 2);
// Now, an AABB
CollisionAABB aabb = new CollisionAABB(new Vector3(9.5f,9.5f,0f),
new Vector3(10.5f,10.5f,4f));
shapes.Add(aabb);
API.AddCollisionShape(aabb, 3);
CollisionOBB obb = new CollisionOBB(new Vector3(0f,10f,2),
new Vector3[3] {
new Vector3(1f,0f,0f),
new Vector3(0f,1f,0f),
new Vector3(0f,0f,1f)},
new Vector3(1f, 1f, 1f));
shapes.Add(obb);
API.AddCollisionShape(obb, 4);
FileStream f = new FileStream("C:\\Junk\\DumpSphereTree.txt", FileMode.Create, FileAccess.Write);
StreamWriter writer = new StreamWriter(f);
API.DumpSphereTree(writer);
API.SphereTreeRoot.VerifySphereTree();
// Now a moving object capsule in the center of the square
CollisionCapsule moCap = new CollisionCapsule(new Vector3(5f,5f,0f),
new Vector3(5f,5f,4f),
1f);
// Remember where the moving object started
Vector3 start = moCap.center;
// Move the moving object toward each of the obstacles
foreach (CollisionShape s in shapes) {
moCap.AddDisplacement(start - moCap.center);
MoveToObject(writer, API, s, moCap);
}
writer.Close();
}
public static float SqrDistSegOBB(Segment rkSeg, CollisionOBB rkBox,
SegOBBParams pf)
{
Segment kLine = new Segment(rkSeg.origin, rkSeg.direction);
float fSqrDistance = SqrDistSegOBBInternal(kLine,rkBox,pf);
if (pf.pfLParam >= 0.0f) {
if (pf.pfLParam <= 1.0f) {
return fSqrDistance;
} else {
fSqrDistance = XSqDistPtOBB(rkSeg.origin + rkSeg.direction,rkBox,pf);
if (pf.useIt)
pf.pfLParam = 1.0f;
return fSqrDistance;
}
} else {
fSqrDistance = XSqDistPtOBB(rkSeg.origin,rkBox,pf);
if (pf.useIt)
pf.pfLParam = 0.0f;
return fSqrDistance;
}
}
public static void Case0(int i0, int i1, int i2, ref Vector3 rkPnt,
Vector3 rkDir, CollisionOBB rkBox,
SegOBBParams pf, ref float rfSqrDistance)
{
float fPmE0 = rkPnt[i0] - rkBox.extents[i0];
float fPmE1 = rkPnt[i1] - rkBox.extents[i1];
float fProd0 = rkDir[i1]*fPmE0;
float fProd1 = rkDir[i0]*fPmE1;
float fDelta, fInvLSqr, fInv;
if (fProd0 >= fProd1) {
// line intersects P[i0] = e[i0]
rkPnt[i0] = rkBox.extents[i0];
float fPpE1 = rkPnt[i1] + rkBox.extents[i1];
fDelta = fProd0 - rkDir[i0]*fPpE1;
if (fDelta >= 0.0f)
{
fInvLSqr = 1.0f/(rkDir[i0]*rkDir[i0] + rkDir[i1]*rkDir[i1]);
rfSqrDistance += fDelta*fDelta*fInvLSqr;
if (pf.useIt) {
rkPnt[i1] = -rkBox.extents[i1];
pf.pfLParam = -(rkDir[i0]*fPmE0+rkDir[i1]*fPpE1)*fInvLSqr;
}
} else {
if (pf.useIt) {
fInv = 1.0f/rkDir[i0];
rkPnt[i1] -= fProd0*fInv;
pf.pfLParam = -fPmE0*fInv;
}
}
} else {
// line intersects P[i1] = e[i1]
rkPnt[i1] = rkBox.extents[i1];
float fPpE0 = rkPnt[i0] + rkBox.extents[i0];
fDelta = fProd1 - rkDir[i1]*fPpE0;
if (fDelta >= 0.0f) {
fInvLSqr = 1.0f/(rkDir[i0]*rkDir[i0] + rkDir[i1]*rkDir[i1]);
rfSqrDistance += fDelta*fDelta*fInvLSqr;
if (pf.useIt) {
rkPnt[i0] = -rkBox.extents[i0];
pf.pfLParam = -(rkDir[i0]*fPpE0+rkDir[i1]*fPmE1)*fInvLSqr;
}
} else {
if (pf.useIt) {
fInv = 1.0f/rkDir[i1];
rkPnt[i0] -= fProd1*fInv;
pf.pfLParam = -fPmE1*fInv;
}
}
}
if (rkPnt[i2] < -rkBox.extents[i2]) {
fDelta = rkPnt[i2] + rkBox.extents[i2];
rfSqrDistance += fDelta*fDelta;
rkPnt[i2] = -rkBox.extents[i2];
}
else if (rkPnt[i2] > rkBox.extents[i2]) {
fDelta = rkPnt[i2] - rkBox.extents[i2];
rfSqrDistance += fDelta*fDelta;
rkPnt[i2] = rkBox.extents[i2];
}
}
public static bool TestCapsuleAABB(CollisionCapsule a, CollisionAABB b, CollisionParms parms)
{
// Convert the AABB into an OBB, then run the OBBOBB test.
// Not the most efficient algorithm but it gets the job
// done.
CollisionOBB newB = new CollisionOBB(b.center,
UnitBasisVectors,
(b.max - b.min) * .5f);
return TestCapsuleOBB(a, newB, parms);
}
// Returns true if sphere s intersects OBB b, false otherwise.
// The point p on the OBB closest to the sphere center is also returned
public static bool TestSphereOBB(CollisionSphere s, CollisionOBB b, out Vector3 p)
{
return (SqDistPointOBB(s.center, b, out p) < s.radius * s.radius);
}
public static bool TestCollisionAABBOBB(CollisionShape s1, CollisionShape s2,
CollisionParms parms)
{
CollisionAABB x1 = (CollisionAABB)s1;
CollisionOBB x2 = (CollisionOBB)s2;
// Convert the AABB into an OBB, then run the OBBOBB test.
// Not the most efficient algorithm but it gets the job
// done.
CollisionOBB newx1 = new CollisionOBB(x1.center,
UnitBasisVectors,
(x1.max - x1.min) * .5f);
return TestOBBOBB(newx1, x2, parms);
}
public static float XSqDistPtOBB(Vector3 rkPoint, CollisionOBB rkBox,
SegOBBParams pf)
{
// compute coordinates of point in box coordinate system
Vector3 kDiff = rkPoint - rkBox.center;
Vector3 kClosest = new Vector3(kDiff.Dot(rkBox.axes[0]),
kDiff.Dot(rkBox.axes[1]),
kDiff.Dot(rkBox.axes[2]));
// project test point onto box
float fSqrDistance = 0.0f;
float fDelta;
for (int i=0; i<3; i++) {
if (kClosest[i] < -rkBox.extents[i]) {
fDelta = kClosest[i] + rkBox.extents[i];
fSqrDistance += fDelta*fDelta;
kClosest[i] = -rkBox.extents[i];
}
else if (kClosest[i] > rkBox.extents[i]) {
fDelta = kClosest[i] - rkBox.extents[i];
fSqrDistance += fDelta*fDelta;
kClosest[i] = rkBox.extents[i];
}
}
if (pf.useIt) {
pf.pfBVec = kClosest;
}
return fSqrDistance;
}
public static bool TestOBBOBBInternal(CollisionOBB a, CollisionOBB b)
{
float ra, rb;
Matrix3 R = Matrix3.Zero;
Matrix3 AbsR = Matrix3.Zero;
// Compute rotation matrix expressing b in a's coordinate frame
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
R[i,j] = a.axes[i].Dot(b.axes[j]);
// Compute translation vector t
Vector3 t = b.center - a.center;
// Bring translation into a's coordinate frame
t = new Vector3(t.Dot(a.axes[0]), t.Dot(a.axes[1]), t.Dot(a.axes[2]));
// Compute common subexpressions. Add in an epsilon term to
// counteract arithmetic errors when two edges are parallel and
// their cross product is (near) null (see text for details)
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
AbsR[i,j] = Math.Abs(R[i,j]) + epsilon;
// Test axes L = A0, L = A1, L = A2
for (int i = 0; i < 3; i++) {
ra = a.extents[i];
rb = b.extents[0] * AbsR[i,0] + b.extents[1] * AbsR[i,1] + b.extents[2] * AbsR[i,2];
if (Math.Abs(t[i]) > ra + rb)
return false;
}
// Test axes L = B0, L = B1, L = B2
for (int i = 0; i < 3; i++) {
ra = a.extents[0] * AbsR[0,i] + a.extents[1] * AbsR[1,i] + a.extents[2] * AbsR[2,i];
rb = b.extents[i];
if (Math.Abs(t[0] * R[0,i] + t[1] * R[1,i] + t[2] * R[2,i]) > ra + rb)
return false;
}
// Test axis L = A0 x B0
ra = a.extents[1] * AbsR[2,0] + a.extents[2] * AbsR[1,0];
rb = b.extents[1] * AbsR[0,2] + b.extents[2] * AbsR[0,1];
if (Math.Abs(t[2] * R[1,0] - t[1] * R[2,0]) > ra + rb)
return false;
// Test axis L = A0 x B1
ra = a.extents[1] * AbsR[2,1] + a.extents[2] * AbsR[1,1];
rb = b.extents[0] * AbsR[0,2] + b.extents[2] * AbsR[0,0];
if (Math.Abs(t[2] * R[1,1] - t[1] * R[2,1]) > ra + rb)
return false;
// Test axis L = A0 x B2
ra = a.extents[1] * AbsR[2,2] + a.extents[2] * AbsR[1,2];
rb = b.extents[0] * AbsR[0,1] + b.extents[1] * AbsR[0,0];
if (Math.Abs(t[2] * R[1,2] - t[1] * R[2,2]) > ra + rb)
return false;
// Test axis L = A1 x B0
ra = a.extents[0] * AbsR[2,0] + a.extents[2] * AbsR[0,0];
rb = b.extents[1] * AbsR[1,2] + b.extents[2] * AbsR[1,1];
if (Math.Abs(t[0] * R[2,0] - t[2] * R[0,0]) > ra + rb)
return false;
// Test axis L = A1 x B1
ra = a.extents[0] * AbsR[2,1] + a.extents[2] * AbsR[0,1];
rb = b.extents[0] * AbsR[1,2] + b.extents[2] * AbsR[1,0];
if (Math.Abs(t[0] * R[2,1] - t[2] * R[0,1]) > ra + rb)
return false;
// Test axis L = A1 x B2
ra = a.extents[0] * AbsR[2,2] + a.extents[2] * AbsR[0,2];
rb = b.extents[0] * AbsR[1,1] + b.extents[1] * AbsR[1,0];
if (Math.Abs(t[0] * R[2,2] - t[2] * R[0,2]) > ra + rb)
return false;
// Test axis L = A2 x B0
ra = a.extents[0] * AbsR[1,0] + a.extents[1] * AbsR[0,0];
rb = b.extents[1] * AbsR[2,2] + b.extents[2] * AbsR[2,1];
if (Math.Abs(t[1] * R[0,0] - t[0] * R[1,0]) > ra + rb)
return false;
// Test axis L = A2 x B1
ra = a.extents[0] * AbsR[1,1] + a.extents[1] * AbsR[0,1];
rb = b.extents[0] * AbsR[2,2] + b.extents[2] * AbsR[2,0];
if (Math.Abs(t[1] * R[0,1] - t[0] * R[1,1]) > ra + rb)
return false;
// Test axis L = A2 x B2
ra = a.extents[0] * AbsR[1,2] + a.extents[1] * AbsR[0,2];
rb = b.extents[0] * AbsR[2,1] + b.extents[1] * AbsR[2,0];
if (Math.Abs(t[1] * R[0,2] - t[0] * R[1,2]) > ra + rb)
return false;
// Since no separating axis found, the OBBs must be intersecting
return true;
}
public static void Case00(int i0, int i1, int i2, ref Vector3 rkPnt,
Vector3 rkDir, CollisionOBB rkBox,
SegOBBParams pf, ref float rfSqrDistance)
{
float fDelta;
if (pf.useIt)
pf.pfLParam = (rkBox.extents[i0] - rkPnt[i0])/rkDir[i0];
rkPnt[i0] = rkBox.extents[i0];
if (rkPnt[i1] < -rkBox.extents[i1]) {
fDelta = rkPnt[i1] + rkBox.extents[i1];
rfSqrDistance += fDelta*fDelta;
rkPnt[i1] = -rkBox.extents[i1];
}
else if (rkPnt[i1] > rkBox.extents[i1]) {
fDelta = rkPnt[i1] - rkBox.extents[i1];
rfSqrDistance += fDelta*fDelta;
rkPnt[i1] = rkBox.extents[i1];
}
if (rkPnt[i2] < -rkBox.extents[i2]) {
fDelta = rkPnt[i2] + rkBox.extents[i2];
rfSqrDistance += fDelta*fDelta;
rkPnt[i2] = -rkBox.extents[i2];
}
else if (rkPnt[i2] > rkBox.extents[i2]) {
fDelta = rkPnt[i2] - rkBox.extents[i2];
rfSqrDistance += fDelta*fDelta;
rkPnt[i2] = rkBox.extents[i2];
}
}
