public override CollisionShape Clone()
{
CollisionAABB rv = new CollisionAABB(min, max);
rv.handle = this.handle;
rv.timeStamp = this.timeStamp;
return(rv);
}
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 PathGenerator(bool logPathGeneration, string modelName, PathObjectType poType, float terrainLevel,
Matrix4 modelTransform, List<CollisionShape> collisionVolumes)
{
this.dumpGrid = logPathGeneration;
this.modelName = modelName;
this.poType = poType;
this.modelWidth = poType.width * oneMeter;
this.cellWidth = poType.gridResolution * modelWidth;
this.boxHeight = poType.height * oneMeter;
this.maxClimbDistance = poType.maxClimbSlope * cellWidth;
this.maxDisjointDistance = poType.maxDisjointDistance * oneMeter;
this.minimumFeatureSize = poType.minimumFeatureSize;
this.terrainLevel = terrainLevel;
this.modelTransform = modelTransform;
this.collisionVolumes = collisionVolumes;
stopwatch = new Stopwatch();
stopwatch.Start();
// Create the collisionAPI object
collisionAPI = new CollisionAPI(false);
// Ugly workaround for a modularity problem do to
// unadvised use of static variables: remember the
// existing state of rendering collision volumes
RenderedNode.RenderState oldRenderState = RenderedNode.renderState;
RenderedNode.renderState = RenderedNode.RenderState.None;
// Form the union of the collision volumes; we don't care
// about Y coordinate, only the X and Z coordinates
Vector3 min = Vector3.Zero;
Vector3 max = Vector3.Zero;
for (int i=0; i<collisionVolumes.Count; i++) {
CollisionShape shape = collisionVolumes[i];
// Add the shape to the sphere tree
collisionAPI.SphereTree.AddCollisionShape(shape);
AxisAlignedBox vol = shape.BoundingBox();
// If this is the first iteration, set up the min and
// max
if (i == 0) {
min = vol.Minimum;
max = vol.Maximum;
min.y = terrainLevel;
max.y = terrainLevel;
continue;
}
// Enlarge the box by the dimensions of the shape
min.x = Math.Min(min.x, vol.Minimum.x);
min.z = Math.Min(min.z, vol.Minimum.z);
max.x = Math.Max(max.x, vol.Maximum.x);
max.z = Math.Max(max.z, vol.Maximum.z);
}
// Restore RenderState
RenderedNode.renderState = oldRenderState;
// Round out the max and min by 4 times the grid
// resolution, so we can just divide to find the
// horizontal and vertical numbers are cells
Vector3 margin = Vector3.UnitScale * 4 * cellWidth;
min -= margin;
max += margin;
// Now adjust the max the min coords so that they are
// exactly a multiple of the grid resolution
min.x = MultipleOfResolution(min.x);
min.z = MultipleOfResolution(min.z);
max.x = MultipleOfResolution(max.x);
max.z = MultipleOfResolution(max.z);
// Set the lower left and upper right corners
lowerLeftCorner = min;
upperRightCorner = max;
// Set the horizontal and vertical counts
xCount = (int)((max.x - min.x) / cellWidth);
zCount = (int)((max.z - min.z) / cellWidth);
// Initial the gridBox
gridBox = new AxisAlignedBox(min, max);
// Allocate the grid
grid = new List<GridCell>[xCount, zCount];
for (int i=0; i<xCount; i++)
for (int j=0; j<zCount; j++)
grid[i,j] = new List<GridCell>();
// Initialize the work list, adding the cell at 0,0 and
// the terrain height as the first member
workList = new List<CellLocation>();
workList.Add(new CellLocation(0, 0, terrainLevel, null));
// Create the moving box at the center of the 0, 0 cell,
// and the MovingObject object that contains the moving box
movingBox = new CollisionAABB(
new Vector3(min.x, terrainLevel, min.z),
new Vector3(min.x + cellWidth, terrainLevel + boxHeight, min.z + terrainLevel));
movingObject = new MovingObject(collisionAPI);
movingObject.AddPart(movingBox);
}
// This method is the callback by which the forests controlled
// by the scene manager tell us about SpeedTrees.
private void AddTreeObstacles(SpeedTreeWrapper tree)
{
// If this tree didn't use to be in range but now is
V3 vp = tree.TreePosition;
Vector3 p = new Vector3(vp.x, vp.y, vp.z);
// Find the tile in question
int tileX, tileZ;
WorldToTileCoords(p, out tileX, out tileZ);
bool oir = InRange(tileXCenter, tileZCenter, tileX, tileZ);
bool nir = InRange(newTileXCenter, newTileZCenter, tileX, tileZ);
// if (MO.DoLog)
// MO.Log(String.Format("For tree at {0}, testing !InRange({1},{2},{3},{4}) = {5} && InRange({6},{7},{8},{9}) = {10}",
// MO.MeterString(p),tileXCenter, tileZCenter, tileX, tileZ, MO.Bool(!oir),
// newTileXCenter, newTileZCenter, tileX, tileZ, MO.Bool(nir)));
if (!oir && nir) {
int tX = TileToArrayIndex(tileX, newTileXCenter);
int tZ = TileToArrayIndex(tileZ, newTileZCenter);
uint count = tree.CollisionObjectCount;
long handle = ComposeHandle(tileX, tileZ);
CollisionShape shape = null;
if (MO.DoLog) {
MO.Log(string.Format("Adding tree at {0}, tiles[{1},{2}] = {3}, tile coords[{4},{5}], obj. new count {6}",
MO.MeterString(p), tX, tZ, tiles[tX,tZ], tileX, tileZ, count));
MO.Log(string.Format("Handle {0}, oldcenter {1}, newcenter {2}",
MO.HandleString(handle), MO.MeterString(tileWorldCenter), MO.MeterString(newTileWorldCenter)));
}
float size = 0f;
float variance = 0f;
tree.GetTreeSize(ref size, ref variance);
float scaleFactor = size / tree.OriginalSize;
for (uint i=0; i<count; i++) {
TreeCollisionObject tco = tree.CollisionObject(i);
Vector3 cp = new Vector3(tco.position.x, tco.position.y, tco.position.z) * scaleFactor + p;
Vector3 cd = new Vector3(tco.dimensions.x, tco.dimensions.y, tco.dimensions.z) * scaleFactor;
switch ((CollisionObjectType)tco.type) {
case CollisionObjectType.ColSphere:
shape = new CollisionSphere(cp, cd.x);
break;
case CollisionObjectType.ColCylinder:
// We treat it as a capsule, but we raise the
// capsule up by the capRadius, and shorten
// the segment by the double the capRadius
Vector3 top = cp;
top.y += cd.y - cd.x * 2f;
cp.y += cd.x;
shape = new CollisionCapsule(cp, top, cd.x);
break;
case CollisionObjectType.ColBox:
Vector3 tp = cp;
tp.x -= cd.x * .5f;
tp.y -= cd.y * .5f;
shape = new CollisionAABB(tp, tp + cd);
break;
}
collisionAPI.AddCollisionShape(shape, handle);
tiles[tX, tZ]++;
objectsAdded++;
if (MO.DoLog) {
MO.Log(string.Format(" tiles[{0},{1}] = {2}, tile at [{3},{4}] after adding shape {5}",
tX, tZ, tiles[tX, tZ], tileX, tileZ, shape.ToString()));
}
}
}
}
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);
}
public static bool TestAABBAABB(CollisionAABB a, CollisionAABB b)
{
// Exit with no intersection if separated along an axis
if (a.max[0] < b.min[0] || a.min[0] > b.max[0])
return false;
if (a.max[1] < b.min[1] || a.min[1] > b.max[1])
return false;
if (a.max[2] < b.min[2] || a.min[2] > b.max[2])
return false;
// Overlapping on all axes means AABBs are intersecting
return true;
}
// Computes the square distance between a point p and an AABB b
public static float SqDistPointAABB(Vector3 p, CollisionAABB b)
{
float sqDist = 0.0f;
for (int i = 0; i < 3; i++) {
// For each axis count any excess distance outside box extents
float v = p[i];
if (v < b.min[i])
sqDist += (b.min[i] - v) * (b.min[i] - v);
if (v > b.max[i])
sqDist += (v - b.max[i]) * (v - b.max[i]);
}
return sqDist;
}
// Given point p, return the point q on or in AABB b, that is closest to p
public static void ClosestPtPointAABB(Vector3 p, CollisionAABB b, out Vector3 q)
{
// For each coordinate axis, if the point coordinate value is
// outside box, clamp it to the box, else keep it as is
q = Vector3.Zero;
for (int i = 0; i < 3; i++) {
float v = p[i];
v = Clamp(v, b.min[i], b.max[i]);
q[i] = v;
}
}
// Test if segment specified by points p0 and p1 intersects AABB b
public static bool TestSegmentAABB(Vector3 p0, Vector3 p1, CollisionAABB b)
{
Vector3 e = b.max - b.center; // Box halflength extents
Vector3 m = (p0 + p1) * 0.5f; // Segment midpoint
Vector3 d = p1 - m; // Segment halflength vector
m = m - b.center; // Translate box and segment to origin
// Try world coordinate axes as separating axes
float adx = Math.Abs(d.x);
if (Math.Abs(m.x) > e.x + adx)
return false;
float ady = Math.Abs(d.y);
if (Math.Abs(m.y) > e.y + ady)
return false;
float adz = Math.Abs(d.z);
if (Math.Abs(m.z) > e.z + adz)
return false;
// Add in an epsilon term to counteract arithmetic errors when segment is
// (near) parallel to a coordinate axis (see text for detail)
adx += epsilon;
ady += epsilon;
adz += epsilon;
// Try cross products of segment direction vector with coordinate axes
if (Math.Abs(m.y * d.z - m.z * d.y) > e.y * adz + e.z * ady)
return false;
if (Math.Abs(m.z * d.x - m.x * d.z) > e.x * adz + e.z * adx)
return false;
if (Math.Abs(m.x * d.y - m.y * d.x) > e.x * ady + e.y * adx)
return false;
// No separating axis found; segment must be overlapping AABB
return true;
}
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 );
}
}
// This method is the callback by which the forests controlled
// by the scene manager tell us about SpeedTrees.
private void AddTreeObstacles(SpeedTreeWrapper tree)
{
// If this tree didn't use to be in range but now is
V3 vp = tree.TreePosition;
Vector3 p = new Vector3(vp.x, vp.y, vp.z);
// Find the tile in question
int tileX, tileZ;
WorldToTileCoords(p, out tileX, out tileZ);
bool oir = InRange(tileXCenter, tileZCenter, tileX, tileZ);
bool nir = InRange(newTileXCenter, newTileZCenter, tileX, tileZ);
// if (MO.DoLog)
// MO.Log(String.Format("For tree at {0}, testing !InRange({1},{2},{3},{4}) = {5} && InRange({6},{7},{8},{9}) = {10}",
// MO.MeterString(p),tileXCenter, tileZCenter, tileX, tileZ, MO.Bool(!oir),
// newTileXCenter, newTileZCenter, tileX, tileZ, MO.Bool(nir)));
if (!oir && nir)
{
int tX = TileToArrayIndex(tileX, newTileXCenter);
int tZ = TileToArrayIndex(tileZ, newTileZCenter);
uint count = tree.CollisionObjectCount;
long handle = ComposeHandle(tileX, tileZ);
CollisionShape shape = null;
if (MO.DoLog)
{
MO.Log(string.Format("Adding tree at {0}, tiles[{1},{2}] = {3}, tile coords[{4},{5}], obj. new count {6}",
MO.MeterString(p), tX, tZ, tiles[tX, tZ], tileX, tileZ, count));
MO.Log(string.Format("Handle {0}, oldcenter {1}, newcenter {2}",
MO.HandleString(handle), MO.MeterString(tileWorldCenter), MO.MeterString(newTileWorldCenter)));
}
float size = 0f;
float variance = 0f;
tree.GetTreeSize(ref size, ref variance);
float scaleFactor = size / tree.OriginalSize;
for (uint i = 0; i < count; i++)
{
TreeCollisionObject tco = tree.CollisionObject(i);
Vector3 cp = new Vector3(tco.position.x, tco.position.y, tco.position.z) * scaleFactor + p;
Vector3 cd = new Vector3(tco.dimensions.x, tco.dimensions.y, tco.dimensions.z) * scaleFactor;
switch ((CollisionObjectType)tco.type)
{
case CollisionObjectType.ColSphere:
shape = new CollisionSphere(cp, cd.x);
break;
case CollisionObjectType.ColCylinder:
// We treat it as a capsule, but we raise the
// capsule up by the capRadius, and shorten
// the segment by the double the capRadius
Vector3 top = cp;
top.y += cd.y - cd.x * 2f;
cp.y += cd.x;
shape = new CollisionCapsule(cp, top, cd.x);
break;
case CollisionObjectType.ColBox:
Vector3 tp = cp;
tp.x -= cd.x * .5f;
tp.y -= cd.y * .5f;
shape = new CollisionAABB(tp, tp + cd);
break;
}
collisionAPI.AddCollisionShape(shape, handle);
tiles[tX, tZ]++;
objectsAdded++;
if (MO.DoLog)
{
MO.Log(string.Format(" tiles[{0},{1}] = {2}, tile at [{3},{4}] after adding shape {5}",
tX, tZ, tiles[tX, tZ], tileX, tileZ, shape.ToString()));
}
}
}
}
public override CollisionShape Clone()
{
CollisionAABB rv = new CollisionAABB(min, max);
rv.handle = this.handle;
rv.timeStamp = this.timeStamp;
return rv;
}
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;
}
