public override void ProcessAllTriangles(ITriangleCallback callback, ref IndexedVector3 aabbMin, ref IndexedVector3 aabbMax)
{
LockChildShapes();
AABB box = new AABB();
box.m_min = aabbMin;
box.m_max = aabbMax;
ObjectArray <int> collided = new ObjectArray <int>();
m_box_set.BoxQuery(ref box, collided);
if (collided.Count == 0)
{
UnlockChildShapes();
return;
}
int part = GetPart();
PrimitiveTriangle triangle = new PrimitiveTriangle();
int i = collided.Count;
while (i-- != 0)
{
GetPrimitiveTriangle(collided[i], triangle);
callback.ProcessTriangle(triangle.m_vertices, part, collided[i]);
}
UnlockChildShapes();
}
public override void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax)
{
Vector3 halfExtents = (aabbMax - aabbMin) * 0.5f;
float radius = halfExtents.Length();
Vector3 center = (aabbMax + aabbMin) * 0.5f;
//this is where the triangles are generated, given AABB and plane equation (normal/constant)
Vector3 tangentDir0 = new Vector3(), tangentDir1 = new Vector3();
//tangentDir0/tangentDir1 can be precalculated
MathHelper.PlaneSpace1(_planeNormal, ref tangentDir0, ref tangentDir1);
Vector3 projectedCenter = center - (Vector3.Dot(_planeNormal, center) - _planeConstant) * _planeNormal;
Vector3[] triangle = new Vector3[3];
triangle[0] = projectedCenter + tangentDir0 * radius + tangentDir1 * radius;
triangle[1] = projectedCenter + tangentDir0 * radius - tangentDir1 * radius;
triangle[2] = projectedCenter - tangentDir0 * radius - tangentDir1 * radius;
callback.ProcessTriangle(triangle, 0, 0);
triangle[0] = projectedCenter - tangentDir0 * radius - tangentDir1 * radius;
triangle[1] = projectedCenter - tangentDir0 * radius + tangentDir1 * radius;
triangle[2] = projectedCenter + tangentDir0 * radius + tangentDir1 * radius;
callback.ProcessTriangle(triangle, 0, 1);
}
public override void ProcessAllTriangles(ITriangleCallback callback, ref IndexedVector3 aabbMin, ref IndexedVector3 aabbMax)
{
IndexedVector3 halfExtents = (aabbMax - aabbMin) * .5f;
float radius = halfExtents.Length();
IndexedVector3 center = (aabbMax + aabbMin) * 0.5f;
//this is where the triangles are generated, given AABB and plane equation (normal/constant)
IndexedVector3 tangentDir0;
IndexedVector3 tangentDir1;
//tangentDir0/tangentDir1 can be precalculated
TransformUtil.PlaneSpace1(ref m_planeNormal, out tangentDir0, out tangentDir1);
IndexedVector3 supVertex0 = IndexedVector3.Zero;
IndexedVector3 supVertex1 = IndexedVector3.Zero;
IndexedVector3 projectedCenter = center - (IndexedVector3.Dot(m_planeNormal, center) - m_planeConstant) * m_planeNormal;
IndexedVector3[] triangle = new IndexedVector3[3];
triangle[0] = (projectedCenter + tangentDir0 * radius + tangentDir1 * radius);
triangle[1] = (projectedCenter + tangentDir0 * radius - tangentDir1 * radius);
triangle[2] = (projectedCenter - tangentDir0 * radius - tangentDir1 * radius);
callback.ProcessTriangle(triangle, 0, 0);
triangle[0] = projectedCenter - tangentDir0 * radius - tangentDir1 * radius;
triangle[1] = projectedCenter - tangentDir0 * radius + tangentDir1 * radius;
triangle[2] = projectedCenter + tangentDir0 * radius + tangentDir1 * radius;
callback.ProcessTriangle(triangle, 0, 1);
}
public override void ProcessAllTriangles(ITriangleCallback callback, ref IndexedVector3 aabbMin, ref IndexedVector3 aabbMax)
{
FilteredCallback filterCallback = new FilteredCallback(callback, ref aabbMin, ref aabbMax);
m_meshInterface.InternalProcessAllTriangles(filterCallback, ref aabbMin, ref aabbMax);
filterCallback.Cleanup();
}
public override void ProcessAllTriangles(ITriangleCallback callback, ref IndexedVector3 aabbMin, ref IndexedVector3 aabbMax)
{
IndexedVector3 halfExtents = (aabbMax - aabbMin) * .5f;
float radius = halfExtents.Length();
IndexedVector3 center = (aabbMax + aabbMin) * 0.5f;
//this is where the triangles are generated, given AABB and plane equation (normal/constant)
IndexedVector3 tangentDir0;
IndexedVector3 tangentDir1;
//tangentDir0/tangentDir1 can be precalculated
TransformUtil.PlaneSpace1(ref m_planeNormal, out tangentDir0, out tangentDir1);
IndexedVector3 supVertex0 = IndexedVector3.Zero;
IndexedVector3 supVertex1 = IndexedVector3.Zero;
IndexedVector3 projectedCenter = center - (IndexedVector3.Dot(m_planeNormal,center) - m_planeConstant)*m_planeNormal;
IndexedVector3[] triangle = new IndexedVector3[3];
triangle[0] = (projectedCenter + tangentDir0*radius + tangentDir1*radius);
triangle[1] = (projectedCenter + tangentDir0 * radius - tangentDir1 * radius);
triangle[2] = (projectedCenter - tangentDir0 * radius - tangentDir1 * radius);
callback.ProcessTriangle(triangle,0,0);
triangle[0] = projectedCenter - tangentDir0*radius - tangentDir1*radius;
triangle[1] = projectedCenter - tangentDir0*radius + tangentDir1*radius;
triangle[2] = projectedCenter + tangentDir0*radius + tangentDir1*radius;
callback.ProcessTriangle(triangle,0,1);
}
//! Function for retrieve triangles.
/*!
* It gives the triangles in local space
*/
public override void ProcessAllTriangles(ITriangleCallback callback, ref IndexedVector3 aabbMin, ref IndexedVector3 aabbMax)
{
int i = m_mesh_parts.Count;
while (i-- != 0)
{
m_mesh_parts[i].ProcessAllTriangles(callback, ref aabbMin, ref aabbMax);
}
}
public MyNodeOverlapCallback(ITriangleCallback callback, StridingMeshInterface meshInterface)
{
m_meshInterface = meshInterface;
m_callback = callback;
// populate with basic data.
m_triangle.Add(Vector3.One);
m_triangle.Add(Vector3.One);
m_triangle.Add(Vector3.One);
}
public void PerformConvexCast(ITriangleCallback callback, ref IndexedVector3 boxSource, ref IndexedVector3 boxTarget, ref IndexedVector3 boxMin, ref IndexedVector3 boxMax)
{
if (m_bvh != null)
{
using (MyNodeOverlapCallback myNodeCallback = BulletGlobals.MyNodeOverlapCallbackPool.Get())
{
myNodeCallback.Initialize(callback, m_meshInterface);
m_bvh.ReportBoxCastOverlappingNodex(myNodeCallback, ref boxSource, ref boxTarget, ref boxMin, ref boxMax);
}
}
}
public void PerformRaycast(ITriangleCallback callback, ref IndexedVector3 raySource, ref IndexedVector3 rayTarget)
{
if (m_bvh != null)
{
using (MyNodeOverlapCallback myNodeCallback = BulletGlobals.MyNodeOverlapCallbackPool.Get())
{
myNodeCallback.Initialize(callback, m_meshInterface);
m_bvh.ReportRayOverlappingNodex(myNodeCallback, ref raySource, ref rayTarget);
}
}
}
//public override void processAllTriangles(ITriangleCallback callback, ref IndexedVector3 aabbMin, ref IndexedVector3 aabbMax)
public override void ProcessAllTriangles(ITriangleCallback callback, ref IndexedVector3 aabbMin, ref IndexedVector3 aabbMax)
{
#if DISABLE_BVH
base.ProcessAllTriangles(callback, ref aabbMin, ref aabbMax);
#else
if (m_bvh != null)
{
using (MyNodeOverlapCallback myNodeCallback = BulletGlobals.MyNodeOverlapCallbackPool.Get())
{
myNodeCallback.Initialize(callback, m_meshInterface);
m_bvh.ReportAabbOverlappingNodex(myNodeCallback, ref aabbMin, ref aabbMax);
}
}
#endif
}
public abstract void ProcessAllTriangles(ITriangleCallback callback,ref IndexedVector3 aabbMin,ref IndexedVector3 aabbMax);
public override void ProcessAllTriangles(ITriangleCallback callback, ref IndexedVector3 vec1, ref IndexedVector3 vec2)
{
}
/// process all triangles within the provided axis-aligned bounding box
/**
basic algorithm:
- convert input aabb to local coordinates (scale down and shift for local origin)
- convert input aabb to a range of heightfield grid points (quantize)
- iterate over all triangles in that subset of the grid
*/
public override void ProcessAllTriangles(ITriangleCallback callback, ref Vector3 aabbMin, ref Vector3 aabbMax)
{
// scale down the input aabb's so they are in local (non-scaled) coordinates
Vector3 invScale = new Vector3(1f, 1f, 1f);
invScale /= m_localScaling;
Vector3 localAabbMin = aabbMin * invScale;
Vector3 localAabbMax = aabbMax * invScale;
// account for local origin
localAabbMin += m_localOrigin;
localAabbMax += m_localOrigin;
//quantize the aabbMin and aabbMax, and adjust the start/end ranges
int[] quantizedAabbMin = new int[3];
int[] quantizedAabbMax = new int[3];
QuantizeWithClamp(quantizedAabbMin, ref localAabbMin, 0);
QuantizeWithClamp(quantizedAabbMax, ref localAabbMax, 1);
// expand the min/max quantized values
// this is to catch the case where the input aabb falls between grid points!
for (int i = 0; i < 3; ++i)
{
quantizedAabbMin[i]--;
quantizedAabbMax[i]++;
}
int startX = 0;
int endX = m_heightStickWidth - 1;
int startJ = 0;
int endJ = m_heightStickLength - 1;
switch (m_upAxis)
{
case 0:
{
if (quantizedAabbMin[1] > startX)
startX = quantizedAabbMin[1];
if (quantizedAabbMax[1] < endX)
endX = quantizedAabbMax[1];
if (quantizedAabbMin[2] > startJ)
startJ = quantizedAabbMin[2];
if (quantizedAabbMax[2] < endJ)
endJ = quantizedAabbMax[2];
break;
}
case 1:
{
if (quantizedAabbMin[0] > startX)
startX = quantizedAabbMin[0];
if (quantizedAabbMax[0] < endX)
endX = quantizedAabbMax[0];
if (quantizedAabbMin[2] > startJ)
startJ = quantizedAabbMin[2];
if (quantizedAabbMax[2] < endJ)
endJ = quantizedAabbMax[2];
break;
};
case 2:
{
if (quantizedAabbMin[0] > startX)
startX = quantizedAabbMin[0];
if (quantizedAabbMax[0] < endX)
endX = quantizedAabbMax[0];
if (quantizedAabbMin[1] > startJ)
startJ = quantizedAabbMin[1];
if (quantizedAabbMax[1] < endJ)
endJ = quantizedAabbMax[1];
break;
}
default:
{
//need to get valid m_upAxis
Debug.Assert(false);
break;
}
}
ObjectArray<Vector3> vertices = new ObjectArray<Vector3>();
Vector3[] tempVectors = new Vector3[3];
for (int j = startJ; j < endJ; j++)
{
for (int x = startX; x < endX; x++)
{
if (m_flipQuadEdges || (m_useDiamondSubdivision && (((j + x) & 1) > 0)))
{
//first triangle
GetVertex(x, j, ref tempVectors[0]);
GetVertex(x + 1, j, ref tempVectors[1]);
GetVertex(x + 1, j + 1, ref tempVectors[2]);
vertices.Clear();
for (int a = 0; a < tempVectors.Length; ++a)
{
vertices.Add(tempVectors[a]);
}
callback.ProcessTriangle(vertices, x, j);
//second triangle
GetVertex(x, j, ref tempVectors[0]);
GetVertex(x + 1, j + 1, ref tempVectors[1]);
GetVertex(x, j + 1, ref tempVectors[2]);
vertices.Clear();
for (int a = 0; a < tempVectors.Length; ++a)
{
vertices.Add(tempVectors[a]);
}
callback.ProcessTriangle(vertices, x, j);
}
else
{
//first triangle
GetVertex(x, j, ref tempVectors[0]);
GetVertex(x, j + 1, ref tempVectors[1]);
GetVertex(x + 1, j, ref tempVectors[2]);
vertices.Clear();
for (int a = 0; a < tempVectors.Length; ++a)
{
vertices.Add(tempVectors[a]);
}
callback.ProcessTriangle(vertices, x, j);
//second triangle
GetVertex(x + 1, j, ref tempVectors[0]);
GetVertex(x, j + 1, ref tempVectors[1]);
GetVertex(x + 1, j + 1, ref tempVectors[2]);
vertices.Clear();
for (int a = 0; a < tempVectors.Length; ++a)
{
vertices.Add(tempVectors[a]);
}
callback.ProcessTriangle(vertices, x, j);
}
}
}
}
public override void ProcessAllTriangles(ITriangleCallback callback, Microsoft.Xna.Framework.Vector3 aabbMin, Microsoft.Xna.Framework.Vector3 aabbMax)
{
throw new Exception("The method or operation is not implemented.");
}
protected void LocalProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax)
{
FilteredCallback filterCallback = new FilteredCallback(callback, aabbMin, aabbMax);
_meshInterface.InternalProcessAllTriangles(filterCallback, aabbMin, aabbMax);
}
public FilteredCallback(ITriangleCallback callback,ref IndexedVector3 aabbMin,ref IndexedVector3 aabbMax)
{
m_callback = callback;
m_aabbMin = aabbMin;
m_aabbMax = aabbMax;
}
public FilteredCallback(ITriangleCallback callback, ref IndexedVector3 aabbMin, ref IndexedVector3 aabbMax)
{
m_callback = callback;
m_aabbMin = aabbMin;
m_aabbMax = aabbMax;
}
public void Initialize(ITriangleCallback callback, StridingMeshInterface meshInterface)
{
m_meshInterface = meshInterface;
m_callback = callback;
}
public MyNodeOverlapCallback() { } // for pool
public MyNodeOverlapCallback(ITriangleCallback callback, StridingMeshInterface meshInterface)
{
m_meshInterface = meshInterface;
m_callback = callback;
}
//public override void processAllTriangles(ITriangleCallback callback, ref IndexedVector3 aabbMin, ref IndexedVector3 aabbMax)
public override void ProcessAllTriangles(ITriangleCallback callback, ref IndexedVector3 aabbMin, ref IndexedVector3 aabbMax)
{
#if DISABLE_BVH
base.ProcessAllTriangles(callback,ref aabbMin,ref aabbMax);
#else
if (m_bvh != null)
{
using (MyNodeOverlapCallback myNodeCallback = BulletGlobals.MyNodeOverlapCallbackPool.Get())
{
myNodeCallback.Initialize(callback, m_meshInterface);
m_bvh.ReportAabbOverlappingNodex(myNodeCallback, ref aabbMin, ref aabbMax);
}
}
#endif
}
public void PerformConvexCast(ITriangleCallback callback, ref IndexedVector3 boxSource, ref IndexedVector3 boxTarget, ref IndexedVector3 boxMin, ref IndexedVector3 boxMax)
{
if (m_bvh != null)
{
using (MyNodeOverlapCallback myNodeCallback = BulletGlobals.MyNodeOverlapCallbackPool.Get())
{
myNodeCallback.Initialize(callback, m_meshInterface);
m_bvh.ReportBoxCastOverlappingNodex(myNodeCallback, ref boxSource, ref boxTarget, ref boxMin, ref boxMax);
}
}
}
public void PerformRaycast(ITriangleCallback callback, ref IndexedVector3 raySource, ref IndexedVector3 rayTarget)
{
if (m_bvh != null)
{
using (MyNodeOverlapCallback myNodeCallback = BulletGlobals.MyNodeOverlapCallbackPool.Get())
{
myNodeCallback.Initialize(callback, m_meshInterface);
m_bvh.ReportRayOverlappingNodex(myNodeCallback, ref raySource, ref rayTarget);
}
}
}
public override void ProcessAllTriangles(ITriangleCallback callback, ref IndexedVector3 aabbMin, ref IndexedVector3 aabbMax)
{
// scale down the input aabb's so they are in local (non-scaled) coordinates
IndexedVector3 invScale = new IndexedVector3(1f) / m_localScaling;
IndexedVector3 localAabbMin = aabbMin * invScale;
IndexedVector3 localAabbMax = aabbMax * invScale;
// account for local origin
localAabbMin += m_localOrigin;
localAabbMax += m_localOrigin;
QuantizeWithClamp(quantizedAabbMin, ref localAabbMin, 0);
QuantizeWithClamp(quantizedAabbMax, ref localAabbMax, 1);
// expand the min/max quantized values
// this is to catch the case where the input aabb falls between grid points!
for (int i = 0; i < 3; ++i)
{
quantizedAabbMin[i]--;
quantizedAabbMax[i]++;
}
int startX = 0;
int endX = m_heightStickWidth - 1;
int startJ = 0;
int endJ = m_heightStickLength - 1;
switch (m_upAxis)
{
case 0:
{
if (quantizedAabbMin[1] > startX)
startX = quantizedAabbMin[1];
if (quantizedAabbMax[1] < endX)
endX = quantizedAabbMax[1];
if (quantizedAabbMin[2] > startJ)
startJ = quantizedAabbMin[2];
if (quantizedAabbMax[2] < endJ)
endJ = quantizedAabbMax[2];
break;
}
case 1:
{
if (quantizedAabbMin[0] > startX)
startX = quantizedAabbMin[0];
if (quantizedAabbMax[0] < endX)
endX = quantizedAabbMax[0];
if (quantizedAabbMin[2] > startJ)
startJ = quantizedAabbMin[2];
if (quantizedAabbMax[2] < endJ)
endJ = quantizedAabbMax[2];
break;
};
case 2:
{
if (quantizedAabbMin[0] > startX)
startX = quantizedAabbMin[0];
if (quantizedAabbMax[0] < endX)
endX = quantizedAabbMax[0];
if (quantizedAabbMin[1] > startJ)
startJ = quantizedAabbMin[1];
if (quantizedAabbMax[1] < endJ)
endJ = quantizedAabbMax[1];
break;
}
default:
{
//need to get valid m_upAxis
Debug.Assert(false);
break;
}
}
// debug draw the boxes?
#if DEBUG_ACCELERATOR
if (BulletGlobals.gDebugDraw != null)
{
IndexedVector3 drawMin = aabbMin;
IndexedVector3 drawMax = aabbMax;
BulletGlobals.gDebugDraw.DrawAabb(drawMin, drawMax, new IndexedVector3(0, 1, 0));
}
#endif
for (int j = startJ; j < endJ; j++)
{
for (int x = startX; x < endX; x++)
{
if (m_flipQuadEdges || (m_useDiamondSubdivision && (((j + x) & 1) > 0)))
{
//first triangle
GetVertex(x, j, out vertices[0]);
GetVertex(x + 1, j, out vertices[1]);
GetVertex(x + 1, j + 1, out vertices[2]);
callback.ProcessTriangle(vertices, x, j);
//second triangle
GetVertex(x, j, out vertices[0]);
GetVertex(x + 1, j + 1, out vertices[1]);
GetVertex(x, j + 1, out vertices[2]);
callback.ProcessTriangle(vertices, x, j);
}
else
{
//first triangle
GetVertex(x, j, out vertices[0]);
GetVertex(x, j + 1, out vertices[1]);
GetVertex(x + 1, j, out vertices[2]);
callback.ProcessTriangle(vertices, x, j);
//second triangle
GetVertex(x + 1, j, out vertices[0]);
GetVertex(x, j + 1, out vertices[1]);
GetVertex(x + 1, j + 1, out vertices[2]);
callback.ProcessTriangle(vertices, x, j);
}
}
}
}
public override void ProcessAllTriangles(ITriangleCallback callback, ref IndexedVector3 aabbMin, ref IndexedVector3 aabbMax)
{
// scale down the input aabb's so they are in local (non-scaled) coordinates
IndexedVector3 invScale = new IndexedVector3(1f) / m_localScaling;
IndexedVector3 localAabbMin = aabbMin * invScale;
IndexedVector3 localAabbMax = aabbMax * invScale;
// account for local origin
localAabbMin += m_localOrigin;
localAabbMax += m_localOrigin;
QuantizeWithClamp(quantizedAabbMin, ref localAabbMin, 0);
QuantizeWithClamp(quantizedAabbMax, ref localAabbMax, 1);
// expand the min/max quantized values
// this is to catch the case where the input aabb falls between grid points!
for (int i = 0; i < 3; ++i)
{
quantizedAabbMin[i]--;
quantizedAabbMax[i]++;
}
int startX = 0;
int endX = m_heightStickWidth - 1;
int startJ = 0;
int endJ = m_heightStickLength - 1;
switch (m_upAxis)
{
case 0:
{
if (quantizedAabbMin[1] > startX)
{
startX = quantizedAabbMin[1];
}
if (quantizedAabbMax[1] < endX)
{
endX = quantizedAabbMax[1];
}
if (quantizedAabbMin[2] > startJ)
{
startJ = quantizedAabbMin[2];
}
if (quantizedAabbMax[2] < endJ)
{
endJ = quantizedAabbMax[2];
}
break;
}
case 1:
{
if (quantizedAabbMin[0] > startX)
{
startX = quantizedAabbMin[0];
}
if (quantizedAabbMax[0] < endX)
{
endX = quantizedAabbMax[0];
}
if (quantizedAabbMin[2] > startJ)
{
startJ = quantizedAabbMin[2];
}
if (quantizedAabbMax[2] < endJ)
{
endJ = quantizedAabbMax[2];
}
break;
};
case 2:
{
if (quantizedAabbMin[0] > startX)
{
startX = quantizedAabbMin[0];
}
if (quantizedAabbMax[0] < endX)
{
endX = quantizedAabbMax[0];
}
if (quantizedAabbMin[1] > startJ)
{
startJ = quantizedAabbMin[1];
}
if (quantizedAabbMax[1] < endJ)
{
endJ = quantizedAabbMax[1];
}
break;
}
default:
{
//need to get valid m_upAxis
Debug.Assert(false);
break;
}
}
// debug draw the boxes?
#if DEBUG_ACCELERATOR
if (BulletGlobals.gDebugDraw != null)
{
IndexedVector3 drawMin = aabbMin;
IndexedVector3 drawMax = aabbMax;
BulletGlobals.gDebugDraw.DrawAabb(drawMin, drawMax, new IndexedVector3(0, 1, 0));
}
#endif
for (int j = startJ; j < endJ; j++)
{
for (int x = startX; x < endX; x++)
{
if (m_flipQuadEdges || (m_useDiamondSubdivision && (((j + x) & 1) > 0)))
{
//first triangle
GetVertex(x, j, out vertices[0]);
GetVertex(x + 1, j, out vertices[1]);
GetVertex(x + 1, j + 1, out vertices[2]);
callback.ProcessTriangle(vertices, x, j);
//second triangle
GetVertex(x, j, out vertices[0]);
GetVertex(x + 1, j + 1, out vertices[1]);
GetVertex(x, j + 1, out vertices[2]);
callback.ProcessTriangle(vertices, x, j);
}
else
{
//first triangle
GetVertex(x, j, out vertices[0]);
GetVertex(x, j + 1, out vertices[1]);
GetVertex(x + 1, j, out vertices[2]);
callback.ProcessTriangle(vertices, x, j);
//second triangle
GetVertex(x + 1, j, out vertices[0]);
GetVertex(x, j + 1, out vertices[1]);
GetVertex(x + 1, j + 1, out vertices[2]);
callback.ProcessTriangle(vertices, x, j);
}
}
}
}
public void PerformRaycast(ITriangleCallback callback, ref IndexedVector3 raySource, ref IndexedVector3 rayTarget)
{
if (!HasAccelerator())
{
// if no accelerator then do the normal process triangles call.
ProcessAllTriangles(callback, ref raySource, ref rayTarget);
}
else
{
// if the rays short then don't go through the acclerator either as the node set
// to check will be small
float rayLength2 = (rayTarget - raySource).LengthSquared();
int checkNode = 0;
if (m_upAxis == 0)
{
checkNode = 1;
}
if (rayLength2 < m_minNodeSize * m_localScaling[checkNode])
{
ProcessAllTriangles(callback, ref raySource, ref rayTarget);
}
else
{
ObjectArray <QuadTreeNode> results = new ObjectArray <QuadTreeNode>();
// scale down the input ray so it is in local (non-scaled) coordinates
IndexedVector3 invScale = new IndexedVector3(1f) / m_localScaling;
IndexedVector3 localRaySource = raySource * invScale;
IndexedVector3 localRayTarget = rayTarget * invScale;
// account for local origin
localRaySource += m_localOrigin;
localRayTarget += m_localOrigin;
IndexedVector3 direction = (localRayTarget - localRaySource);
direction.Normalize();
// build list of squares
QueryNode(m_rootQuadTreeNode, results, localRaySource, direction);
// go through each of the results.
foreach (QuadTreeNode quadTreeNode in results)
{
int startX = 0;
int endX = m_heightStickWidth - 1;
int startJ = 0;
int endJ = m_heightStickLength - 1;
IndexedVector3 iv3Min = quadTreeNode.vmin;
IndexedVector3 iv3Max = quadTreeNode.vmax;
switch (m_upAxis)
{
case 0:
{
if (iv3Min.Y > startX)
{
startX = (int)iv3Min.Y;
}
if (iv3Max.Y < endX)
{
endX = (int)iv3Max.Y;
}
if (iv3Min.Z > startJ)
{
startJ = (int)iv3Min.Z;
}
if (iv3Max.Z < endJ)
{
endJ = (int)iv3Max.Z;
}
break;
}
case 1:
{
if (iv3Min.X > startX)
{
startX = (int)iv3Min.X;
}
if (iv3Max.X < endX)
{
endX = (int)iv3Max.X;
}
if (iv3Min.Z > startJ)
{
startJ = (int)iv3Min.Z;
}
if (iv3Max.Z < endJ)
{
endJ = (int)iv3Max.Z;
}
break;
};
case 2:
{
if (iv3Min.X > startX)
{
startX = (int)iv3Min.X;
}
if (iv3Max.X < endX)
{
endX = (int)iv3Max.X;
}
if (iv3Min.Y > startJ)
{
startJ = (int)iv3Min.Y;
}
if (iv3Max.Y < endJ)
{
endJ = (int)iv3Max.Y;
}
break;
}
default:
{
//need to get valid m_upAxis
Debug.Assert(false);
break;
}
}
// debug draw the boxes?
#if DEBUG_ACCELERATOR
if (BulletGlobals.gDebugDraw != null)
{
IndexedVector3 drawMin = iv3Min;
IndexedVector3 drawMax = iv3Max;
IndexedVector3 worldMin = LocalToWorld2(drawMin);
IndexedVector3 worldMax = LocalToWorld2(drawMax);
BulletGlobals.gDebugDraw.DrawAabb(worldMin, worldMax, new IndexedVector3(1, 1, 0));
}
#endif
IndexedVector3[] vertices = new IndexedVector3[3];
for (int j = startJ; j < endJ; j++)
{
for (int x = startX; x < endX; x++)
{
if (m_flipQuadEdges || (m_useDiamondSubdivision && (((j + x) & 1) > 0)))
{
//first triangle
GetVertex(x, j, out vertices[0]);
GetVertex(x + 1, j, out vertices[1]);
GetVertex(x + 1, j + 1, out vertices[2]);
callback.ProcessTriangle(vertices, x, j);
//second triangle
GetVertex(x, j, out vertices[0]);
GetVertex(x + 1, j + 1, out vertices[1]);
GetVertex(x, j + 1, out vertices[2]);
callback.ProcessTriangle(vertices, x, j);
}
else
{
//first triangle
GetVertex(x, j, out vertices[0]);
GetVertex(x, j + 1, out vertices[1]);
GetVertex(x + 1, j, out vertices[2]);
callback.ProcessTriangle(vertices, x, j);
//second triangle
GetVertex(x + 1, j, out vertices[0]);
GetVertex(x, j + 1, out vertices[1]);
GetVertex(x + 1, j + 1, out vertices[2]);
callback.ProcessTriangle(vertices, x, j);
}
}
}
}
}
}
}
public override void ProcessAllTriangles(ITriangleCallback callback, MonoXnaCompactMaths.Vector3 aabbMin, MonoXnaCompactMaths.Vector3 aabbMax)
{
throw new Exception("The method or operation is not implemented.");
}
public override void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax)
{
MyNodeOverlapCallback myNodeCallback = new MyNodeOverlapCallback(callback, MeshInterface);
_bvh.ReportAabbOverlappingNodex(myNodeCallback, aabbMin, aabbMax);
}
public override void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax)
{
LocalProcessAllTriangles(callback, aabbMax, aabbMax);
}
public override void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax)
{
LocalProcessAllTriangles(callback, aabbMax, aabbMax);
}
protected void LocalProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax)
{
FilteredCallback filterCallback = new FilteredCallback(callback, aabbMin, aabbMax);
_meshInterface.InternalProcessAllTriangles(filterCallback, aabbMin, aabbMax);
}
public FilteredCallback(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax)
{
_callback = callback;
_aabbMin = aabbMin;
_aabbMax = aabbMax;
}
//! Function for retrieve triangles.
/*!
It gives the triangles in local space
*/
public override void ProcessAllTriangles(ITriangleCallback callback, ref IndexedVector3 aabbMin, ref IndexedVector3 aabbMax)
{
int i = m_mesh_parts.Count;
while (i-- != 0)
{
m_mesh_parts[i].ProcessAllTriangles(callback, ref aabbMin, ref aabbMax);
}
}
public override void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax) {
Vector3 halfExtents = (aabbMax - aabbMin) * 0.5f;
float radius = halfExtents.Length();
Vector3 center = (aabbMax + aabbMin) * 0.5f;
//this is where the triangles are generated, given AABB and plane equation (normal/constant)
Vector3 tangentDir0 = new Vector3(), tangentDir1 = new Vector3();
//tangentDir0/tangentDir1 can be precalculated
MathHelper.PlaneSpace1(_planeNormal, ref tangentDir0, ref tangentDir1);
Vector3 projectedCenter = center - (Vector3.Dot(_planeNormal, center) - _planeConstant) * _planeNormal;
Vector3[] triangle = new Vector3[3];
triangle[0] = projectedCenter + tangentDir0 * radius + tangentDir1 * radius;
triangle[1] = projectedCenter + tangentDir0 * radius - tangentDir1 * radius;
triangle[2] = projectedCenter - tangentDir0 * radius - tangentDir1 * radius;
callback.ProcessTriangle(triangle, 0, 0);
triangle[0] = projectedCenter - tangentDir0 * radius - tangentDir1 * radius;
triangle[1] = projectedCenter - tangentDir0 * radius + tangentDir1 * radius;
triangle[2] = projectedCenter + tangentDir0 * radius + tangentDir1 * radius;
callback.ProcessTriangle(triangle, 0, 1);
}
//! Function for retrieve triangles.
/*!
It gives the triangles in local space
*/
public override void ProcessAllTriangles(ITriangleCallback callback, ref IndexedVector3 aabbMin, ref IndexedVector3 aabbMax)
{
}
public FilteredCallback(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax)
{
_callback = callback;
_aabbMin = aabbMin;
_aabbMax = aabbMax;
}
public override void ProcessAllTriangles(ITriangleCallback callback, ref IndexedVector3 aabbMin, ref IndexedVector3 aabbMax)
{
LockChildShapes();
AABB box = new AABB();
box.m_min = aabbMin;
box.m_max = aabbMax;
ObjectArray<int> collided = new ObjectArray<int>();
m_box_set.BoxQuery(ref box, collided);
if (collided.Count == 0)
{
UnlockChildShapes();
return;
}
int part = GetPart();
PrimitiveTriangle triangle = new PrimitiveTriangle();
int i = collided.Count;
while (i-- != 0)
{
GetPrimitiveTriangle(collided[i], triangle);
callback.ProcessTriangle(triangle.m_vertices, part, collided[i]);
}
UnlockChildShapes();
}
public override void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax)
{
//(void)callback;
//(void)aabbMax;
//(void)aabbMin;
//quantize the aabbMin and aabbMax, and adjust the start/end ranges
int[] quantizedAabbMin = new int[3];
int[] quantizedAabbMax = new int[3];
Vector3 localAabbMin = aabbMin * new Vector3(1f / _localScaling.X, 1f / _localScaling.Y, 1f / _localScaling.Z);
Vector3 localAabbMax = aabbMax * new Vector3(1f / _localScaling.X, 1f / _localScaling.Y, 1f / _localScaling.Z);
quantizeWithClamp(ref quantizedAabbMin, localAabbMin);
quantizeWithClamp(ref quantizedAabbMax, localAabbMax);
int startX = 0;
int endX = _width - 1;
int startJ = 0;
int endJ = _length - 1;
switch (_upAxis)
{
case 0:
quantizedAabbMin[1] += _width / 2 - 1;
quantizedAabbMax[1] += _width / 2 + 1;
quantizedAabbMin[2] += _length / 2 - 1;
quantizedAabbMax[2] += _length / 2 + 1;
if (quantizedAabbMin[1] > startX)
{
startX = quantizedAabbMin[1];
}
if (quantizedAabbMax[1] < endX)
{
endX = quantizedAabbMax[1];
}
if (quantizedAabbMin[2] > startJ)
{
startJ = quantizedAabbMin[2];
}
if (quantizedAabbMax[2] < endJ)
{
endJ = quantizedAabbMax[2];
}
break;
case 1:
quantizedAabbMin[0] += _width / 2 - 1;
quantizedAabbMax[0] += _width / 2 + 1;
quantizedAabbMin[2] += _length / 2 - 1;
quantizedAabbMax[2] += _length / 2 + 1;
if (quantizedAabbMin[0] > startX)
{
startX = quantizedAabbMin[0];
}
if (quantizedAabbMax[0] < endX)
{
endX = quantizedAabbMax[0];
}
if (quantizedAabbMin[2] > startJ)
{
startJ = quantizedAabbMin[2];
}
if (quantizedAabbMax[2] < endJ)
{
endJ = quantizedAabbMax[2];
}
break;
case 2:
quantizedAabbMin[0] += _width / 2 - 1;
quantizedAabbMax[0] += _width / 2 + 1;
quantizedAabbMin[1] += _length / 2 - 1;
quantizedAabbMax[1] += _length / 2 + 1;
if (quantizedAabbMin[0] > startX)
{
startX = quantizedAabbMin[0];
}
if (quantizedAabbMax[0] < endX)
{
endX = quantizedAabbMax[0];
}
if (quantizedAabbMin[1] > startJ)
{
startJ = quantizedAabbMin[1];
}
if (quantizedAabbMax[1] < endJ)
{
endJ = quantizedAabbMax[1];
}
break;
default:
//need to get valid m_upAxis
throw new Exception("HeightfieldTerrainShape: need to get valid _upAxis");
//break;
}
for (int j = startJ; j < endJ; j++)
{
for (int x = startX; x < endX; x++)
{
Vector3[] vertices = new Vector3[3];
//if (m_flipQuadEdges || (m_useDiamondSubdivision && ((j + x) & 1)))
if (_flipQuadEdges || (_useDiamondSubdivision && (((j + x) & 1) > 0)))
{
//first triangle
getVertex(x, j, ref vertices[0]);
getVertex(x + 1, j, ref vertices[1]);
getVertex(x + 1, j + 1, ref vertices[2]);
//callback->processTriangle(vertices,x,j);
callback.ProcessTriangle(vertices, x, j);
//second triangle
getVertex(x, j, ref vertices[0]);
getVertex(x + 1, j + 1, ref vertices[1]);
getVertex(x, j + 1, ref vertices[2]);
//callback->processTriangle(vertices,x,j);
callback.ProcessTriangle(vertices, x, j);
}
else
{
//first triangle
getVertex(x, j, ref vertices[0]);
getVertex(x, j + 1, ref vertices[1]);
getVertex(x + 1, j, ref vertices[2]);
//callback->processTriangle(vertices,x,j);
callback.ProcessTriangle(vertices, x, j);
//second triangle
getVertex(x + 1, j, ref vertices[0]);
getVertex(x, j + 1, ref vertices[1]);
getVertex(x + 1, j + 1, ref vertices[2]);
//callback->processTriangle(vertices,x,j);
callback.ProcessTriangle(vertices, x, j);
}
}
}
}
public abstract void ProcessAllTriangles(ITriangleCallback callback, ref IndexedVector3 aabbMin, ref IndexedVector3 aabbMax);
public override void ProcessAllTriangles(ITriangleCallback callback, MonoXnaCompactMaths.Vector3 aabbMin, MonoXnaCompactMaths.Vector3 aabbMax)
{
throw new Exception("The method or operation is not implemented.");
}
public void Initialize(ITriangleCallback callback, StridingMeshInterface meshInterface)
{
m_meshInterface = meshInterface;
m_callback = callback;
}
//! Function for retrieve triangles.
/*!
* It gives the triangles in local space
*/
public override void ProcessAllTriangles(ITriangleCallback callback, ref IndexedVector3 aabbMin, ref IndexedVector3 aabbMax)
{
}
public override void ProcessAllTriangles(ITriangleCallback callback, ref IndexedVector3 vec1, ref IndexedVector3 vec2)
{
}
} // for pool
public MyNodeOverlapCallback(ITriangleCallback callback, StridingMeshInterface meshInterface)
{
m_meshInterface = meshInterface;
m_callback = callback;
}
public void PerformRaycast(ITriangleCallback callback, ref IndexedVector3 raySource, ref IndexedVector3 rayTarget)
{
if (!HasAccelerator())
{
// if no accelerator then do the normal process triangles call.
ProcessAllTriangles(callback, ref raySource, ref rayTarget);
}
else
{
// if the rays short then don't go through the acclerator either as the node set
// to check will be small
float rayLength2 = (rayTarget - raySource).LengthSquared();
int checkNode = 0;
if (m_upAxis == 0)
{
checkNode = 1;
}
if (rayLength2 < m_minNodeSize * m_localScaling[checkNode])
{
ProcessAllTriangles(callback, ref raySource, ref rayTarget);
}
else
{
ObjectArray<QuadTreeNode> results = new ObjectArray<QuadTreeNode>();
// scale down the input ray so it is in local (non-scaled) coordinates
IndexedVector3 invScale = new IndexedVector3(1f) / m_localScaling;
IndexedVector3 localRaySource = raySource * invScale;
IndexedVector3 localRayTarget = rayTarget * invScale;
// account for local origin
localRaySource += m_localOrigin;
localRayTarget += m_localOrigin;
IndexedVector3 direction = (localRayTarget - localRaySource);
direction.Normalize();
// build list of squares
QueryNode(m_rootQuadTreeNode, results, localRaySource, direction);
// go through each of the results.
foreach (QuadTreeNode quadTreeNode in results)
{
int startX = 0;
int endX = m_heightStickWidth - 1;
int startJ = 0;
int endJ = m_heightStickLength - 1;
IndexedVector3 iv3Min = quadTreeNode.vmin;
IndexedVector3 iv3Max = quadTreeNode.vmax;
switch (m_upAxis)
{
case 0:
{
if (iv3Min.Y > startX)
startX = (int)iv3Min.Y;
if (iv3Max.Y < endX)
endX = (int)iv3Max.Y;
if (iv3Min.Z > startJ)
startJ = (int)iv3Min.Z;
if (iv3Max.Z < endJ)
endJ = (int)iv3Max.Z;
break;
}
case 1:
{
if (iv3Min.X > startX)
startX = (int)iv3Min.X;
if (iv3Max.X < endX)
endX = (int)iv3Max.X;
if (iv3Min.Z > startJ)
startJ = (int)iv3Min.Z;
if (iv3Max.Z < endJ)
endJ = (int)iv3Max.Z;
break;
};
case 2:
{
if (iv3Min.X > startX)
startX = (int)iv3Min.X;
if (iv3Max.X < endX)
endX = (int)iv3Max.X;
if (iv3Min.Y > startJ)
startJ = (int)iv3Min.Y;
if (iv3Max.Y < endJ)
endJ = (int)iv3Max.Y;
break;
}
default:
{
//need to get valid m_upAxis
Debug.Assert(false);
break;
}
}
// debug draw the boxes?
#if DEBUG_ACCELERATOR
if (BulletGlobals.gDebugDraw != null)
{
IndexedVector3 drawMin = iv3Min;
IndexedVector3 drawMax = iv3Max;
IndexedVector3 worldMin = LocalToWorld2(drawMin);
IndexedVector3 worldMax = LocalToWorld2(drawMax);
BulletGlobals.gDebugDraw.DrawAabb(worldMin, worldMax, new IndexedVector3(1, 1, 0));
}
#endif
IndexedVector3[] vertices = new IndexedVector3[3];
for (int j = startJ; j < endJ; j++)
{
for (int x = startX; x < endX; x++)
{
if (m_flipQuadEdges || (m_useDiamondSubdivision && (((j + x) & 1) > 0)))
{
//first triangle
GetVertex(x, j, out vertices[0]);
GetVertex(x + 1, j, out vertices[1]);
GetVertex(x + 1, j + 1, out vertices[2]);
callback.ProcessTriangle(vertices, x, j);
//second triangle
GetVertex(x, j, out vertices[0]);
GetVertex(x + 1, j + 1, out vertices[1]);
GetVertex(x, j + 1, out vertices[2]);
callback.ProcessTriangle(vertices, x, j);
}
else
{
//first triangle
GetVertex(x, j, out vertices[0]);
GetVertex(x, j + 1, out vertices[1]);
GetVertex(x + 1, j, out vertices[2]);
callback.ProcessTriangle(vertices, x, j);
//second triangle
GetVertex(x + 1, j, out vertices[0]);
GetVertex(x, j + 1, out vertices[1]);
GetVertex(x + 1, j + 1, out vertices[2]);
callback.ProcessTriangle(vertices, x, j);
}
}
}
}
}
}
}
public override void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax)
{
MyNodeOverlapCallback myNodeCallback = new MyNodeOverlapCallback(callback, MeshInterface);
_bvh.ReportAabbOverlappingNodex(myNodeCallback, aabbMin, aabbMax);
}
public override void ProcessAllTriangles(ITriangleCallback callback, ref IndexedVector3 aabbMin, ref IndexedVector3 aabbMax)
{
FilteredCallback filterCallback = new FilteredCallback(callback,ref aabbMin,ref aabbMax);
m_meshInterface.InternalProcessAllTriangles(filterCallback,ref aabbMin,ref aabbMax);
filterCallback.Cleanup();
}
public override void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax)
{
//(void)callback;
//(void)aabbMax;
//(void)aabbMin;
//quantize the aabbMin and aabbMax, and adjust the start/end ranges
int[] quantizedAabbMin = new int[3];
int[] quantizedAabbMax = new int[3];
Vector3 localAabbMin = aabbMin * new Vector3(1f/_localScaling.X,1f/_localScaling.Y,1f/_localScaling.Z );
Vector3 localAabbMax = aabbMax * new Vector3(1f/_localScaling.X,1f/_localScaling.Y,1f/_localScaling.Z);
quantizeWithClamp(ref quantizedAabbMin, localAabbMin);
quantizeWithClamp(ref quantizedAabbMax, localAabbMax);
int startX=0;
int endX=_width-1;
int startJ=0;
int endJ=_length-1;
switch(_upAxis)
{
case 0:
quantizedAabbMin[1]+=_width/2-1;
quantizedAabbMax[1]+=_width/2+1;
quantizedAabbMin[2]+=_length/2-1;
quantizedAabbMax[2]+=_length/2+1;
if (quantizedAabbMin[1]>startX)
startX = quantizedAabbMin[1];
if (quantizedAabbMax[1]<endX)
endX = quantizedAabbMax[1];
if (quantizedAabbMin[2]>startJ)
startJ = quantizedAabbMin[2];
if (quantizedAabbMax[2]<endJ)
endJ = quantizedAabbMax[2];
break;
case 1:
quantizedAabbMin[0]+=_width/2-1;
quantizedAabbMax[0]+=_width/2+1;
quantizedAabbMin[2]+=_length/2-1;
quantizedAabbMax[2]+=_length/2+1;
if (quantizedAabbMin[0]>startX)
startX = quantizedAabbMin[0];
if (quantizedAabbMax[0]<endX)
endX = quantizedAabbMax[0];
if (quantizedAabbMin[2]>startJ)
startJ = quantizedAabbMin[2];
if (quantizedAabbMax[2]<endJ)
endJ = quantizedAabbMax[2];
break;
case 2:
quantizedAabbMin[0]+=_width/2-1;
quantizedAabbMax[0]+=_width/2+1;
quantizedAabbMin[1]+=_length/2-1;
quantizedAabbMax[1]+=_length/2+1;
if (quantizedAabbMin[0]>startX)
startX = quantizedAabbMin[0];
if (quantizedAabbMax[0]<endX)
endX = quantizedAabbMax[0];
if (quantizedAabbMin[1]>startJ)
startJ = quantizedAabbMin[1];
if (quantizedAabbMax[1]<endJ)
endJ = quantizedAabbMax[1];
break;
default:
//need to get valid m_upAxis
throw new Exception("HeightfieldTerrainShape: need to get valid _upAxis");
//break;
}
for(int j=startJ; j<endJ; j++)
{
for(int x=startX; x<endX; x++)
{
Vector3[] vertices = new Vector3[3];
//if (m_flipQuadEdges || (m_useDiamondSubdivision && ((j + x) & 1)))
if (_flipQuadEdges || (_useDiamondSubdivision && (((j + x) & 1) > 0)))
{
//first triangle
getVertex(x,j,ref vertices[0]);
getVertex(x+1,j,ref vertices[1]);
getVertex(x+1,j+1,ref vertices[2]);
//callback->processTriangle(vertices,x,j);
callback.ProcessTriangle(vertices,x,j);
//second triangle
getVertex(x,j,ref vertices[0]);
getVertex(x+1,j+1,ref vertices[1]);
getVertex(x,j+1,ref vertices[2]);
//callback->processTriangle(vertices,x,j);
callback.ProcessTriangle(vertices, x, j);
}
else
{
//first triangle
getVertex(x,j,ref vertices[0]);
getVertex(x,j+1,ref vertices[1]);
getVertex(x+1,j,ref vertices[2]);
//callback->processTriangle(vertices,x,j);
callback.ProcessTriangle(vertices,x,j);
//second triangle
getVertex(x+1,j,ref vertices[0]);
getVertex(x,j+1,ref vertices[1]);
getVertex(x+1,j+1,ref vertices[2]);
//callback->processTriangle(vertices,x,j);
callback.ProcessTriangle(vertices,x,j);
}
}
}
}
public override void ProcessAllTriangles(ITriangleCallback callback, Microsoft.Xna.Framework.Vector3 aabbMin, Microsoft.Xna.Framework.Vector3 aabbMax)
{
throw new Exception("The method or operation is not implemented.");
}
