public void Clone(Shape sh)
{
this.inertia = sh.inertia;
this.mass = sh.mass;
this.boundingBox = sh.boundingBox;
this.geomCen = sh.geomCen;
if (sh is BoxShape)
{
CloneBox((BoxShape)sh);
}
else if (sh is SphereShape)
{
CloneSphere((SphereShape)sh);
}
else if (sh is ConeShape)
{
CloneCone((ConeShape)sh);
}
else if (sh is CylinderShape)
{
CloneCylinder((CylinderShape)sh);
}
else if (sh is CapsuleShape)
{
CloneCapsule((CapsuleShape)sh);
}
}
/// <summary>
/// Recalculates the axis aligned bounding box and the inertia
/// values in world space.
/// </summary>
public virtual void Update()
{
if (isParticle)
{
this.inertia = FPMatrix.Zero;
this.invInertia = this.invInertiaWorld = FPMatrix.Zero;
this.invOrientation = this.orientation = FPMatrix.Identity;
this.boundingBox = shape.boundingBox;
FPVector.Add(ref boundingBox.min, ref this.position, out boundingBox.min);
FPVector.Add(ref boundingBox.max, ref this.position, out boundingBox.max);
angularVelocity.MakeZero();
}
else
{
// Given: Orientation, Inertia
FPMatrix.Transpose(ref orientation, out invOrientation);
this.Shape.GetBoundingBox(ref orientation, out boundingBox);
FPVector.Add(ref boundingBox.min, ref this.position, out boundingBox.min);
FPVector.Add(ref boundingBox.max, ref this.position, out boundingBox.max);
if (!isStatic)
{
FPMatrix.Multiply(ref invOrientation, ref invInertia, out invInertiaWorld);
FPMatrix.Multiply(ref invInertiaWorld, ref orientation, out invInertiaWorld);
}
}
}
/// <summary>
/// Creates a new box containing the two given ones.
/// </summary>
/// <param name="original">First box.</param>
/// <param name="additional">Second box.</param>
/// <returns>A JBBox containing the two given boxes.</returns>
#region public static JBBox CreateMerged(JBBox original, JBBox additional)
public static FPBBox CreateMerged(FPBBox original, FPBBox additional)
{
FPBBox result;
FPBBox.CreateMerged(ref original, ref additional, out result);
return(result);
}
/// <summary>
/// Uses the supportMapping to calculate the bounding box. Should be overidden
/// to make this faster.
/// </summary>
/// <param name="orientation">The orientation of the shape.</param>
/// <param name="box">The resulting axis aligned bounding box.</param>
public virtual void GetBoundingBox(ref FPMatrix orientation, out FPBBox box)
{
// I don't think that this can be done faster.
// 6 is the minimum number of SupportMap calls.
FPVector vec = FPVector.zero;
vec.Set(orientation.M11, orientation.M21, orientation.M31);
SupportMapping(ref vec, out vec);
box.max.x = orientation.M11 * vec.x + orientation.M21 * vec.y + orientation.M31 * vec.z;
vec.Set(orientation.M12, orientation.M22, orientation.M32);
SupportMapping(ref vec, out vec);
box.max.y = orientation.M12 * vec.x + orientation.M22 * vec.y + orientation.M32 * vec.z;
vec.Set(orientation.M13, orientation.M23, orientation.M33);
SupportMapping(ref vec, out vec);
box.max.z = orientation.M13 * vec.x + orientation.M23 * vec.y + orientation.M33 * vec.z;
vec.Set(-orientation.M11, -orientation.M21, -orientation.M31);
SupportMapping(ref vec, out vec);
box.min.x = orientation.M11 * vec.x + orientation.M21 * vec.y + orientation.M31 * vec.z;
vec.Set(-orientation.M12, -orientation.M22, -orientation.M32);
SupportMapping(ref vec, out vec);
box.min.y = orientation.M12 * vec.x + orientation.M22 * vec.y + orientation.M32 * vec.z;
vec.Set(-orientation.M13, -orientation.M23, -orientation.M33);
SupportMapping(ref vec, out vec);
box.min.z = orientation.M13 * vec.x + orientation.M23 * vec.y + orientation.M33 * vec.z;
}
/// <summary>
/// Move a proxy with a swepted AABB. If the proxy has moved outside of its fattened AABB,
/// then the proxy is removed from the tree and re-inserted. Otherwise
/// the function returns immediately.
/// </summary>
/// <param name="proxyId">The proxy id.</param>
/// <param name="aabb">The aabb.</param>
/// <param name="displacement">The displacement.</param>
/// <returns>true if the proxy was re-inserted.</returns>
public bool MoveProxy(int proxyId, ref FPBBox aabb, FPVector displacement)
{
Debug.Assert(0 <= proxyId && proxyId < _nodeCapacity);
Debug.Assert(_nodes[proxyId].IsLeaf());
if (_nodes[proxyId].AABB.Contains(ref aabb) != FPBBox.ContainmentType.Disjoint)
{
return(false);
}
RemoveLeaf(proxyId);
// Extend AABB.
FPBBox b = aabb;
FPVector r = new FPVector(_nodes[proxyId].MinorRandomExtension);
b.min = b.min - r;
b.max = b.max + r;
// Predict AABB displacement.
FPVector d = SettingsAABBMultiplier * displacement;
//JVector randomExpansion = new JVector((FP)rnd.Next(0, 10) * FP.EN1, (FP)rnd.Next(0, 10) * FP.EN1, (FP)rnd.Next(0, 10) * FP.EN1);
//d += randomExpansion;
if (d.x < FP.Zero)
{
b.min.x += d.x;
}
else
{
b.max.x += d.x;
}
if (d.y < FP.Zero)
{
b.min.y += d.y;
}
else
{
b.max.y += d.y;
}
if (d.z < FP.Zero)
{
b.min.z += d.z;
}
else
{
b.max.z += d.z;
}
_nodes[proxyId].AABB = b;
InsertLeaf(proxyId);
return(true);
}
private void RemoveLeaf(int leaf)
{
if (leaf == _root)
{
_root = NullNode;
return;
}
int parent = _nodes[leaf].ParentOrNext;
int grandParent = _nodes[parent].ParentOrNext;
int sibling;
if (_nodes[parent].Child1 == leaf)
{
sibling = _nodes[parent].Child2;
}
else
{
sibling = _nodes[parent].Child1;
}
if (grandParent != NullNode)
{
// Destroy parent and connect sibling to grandParent.
if (_nodes[grandParent].Child1 == parent)
{
_nodes[grandParent].Child1 = sibling;
}
else
{
_nodes[grandParent].Child2 = sibling;
}
_nodes[sibling].ParentOrNext = grandParent;
FreeNode(parent);
// Adjust ancestor bounds.
parent = grandParent;
while (parent != NullNode)
{
//_nodes[parent].AABB.Combine(ref _nodes[_nodes[parent].Child1].AABB,
// ref _nodes[_nodes[parent].Child2].AABB);
FPBBox.CreateMerged(ref _nodes[_nodes[parent].Child1].AABB,
ref _nodes[_nodes[parent].Child2].AABB, out _nodes[parent].AABB);
Debug.Assert(_nodes[parent].LeafCount > 0);
_nodes[parent].LeafCount -= 1;
parent = _nodes[parent].ParentOrNext;
}
}
else
{
_root = sibling;
_nodes[sibling].ParentOrNext = NullNode;
FreeNode(parent);
}
}
/// <summary>
///
/// </summary>
/// <param name="rayOrigin"></param>
/// <param name="rayEnd"></param>
/// <returns></returns>
public override int Prepare(ref FPVector rayOrigin, ref FPVector rayEnd)
{
FPBBox box = FPBBox.SmallBox;
box.AddPoint(ref rayOrigin);
box.AddPoint(ref rayEnd);
return(this.Prepare(ref box));
}
/// <summary>
/// Calculates the bounding box of the sphere.
/// </summary>
/// <param name="orientation">The orientation of the shape.</param>
/// <param name="box">The resulting axis aligned bounding box.</param>
public override void GetBoundingBox(ref FPMatrix orientation, out FPBBox box)
{
box.min.x = -radius;
box.min.y = -radius;
box.min.z = -radius;
box.max.x = radius;
box.max.y = radius;
box.max.z = radius;
}
/// <summary>
/// Checks the AABB of the two rigid bodies.
/// </summary>
/// <param name="entity1">The first body.</param>
/// <param name="entity2">The second body.</param>
/// <returns>Returns true if an intersection occours.</returns>
public bool CheckBoundingBoxes(IBroadphaseEntity entity1, IBroadphaseEntity entity2)
{
FPBBox box1 = entity1.BoundingBox;
FPBBox box2 = entity2.BoundingBox;
return((((box1.max.z >= box2.min.z) && (box1.min.z <= box2.max.z)) &&
((box1.max.y >= box2.min.y) && (box1.min.y <= box2.max.y))) &&
((box1.max.x >= box2.min.x) && (box1.min.x <= box2.max.x)));
}
/// <summary>
/// Creates a new instance of the TransformedShape struct.
/// </summary>
/// <param name="shape">The shape.</param>
/// <param name="orientation">The orientation this shape should have.</param>
/// <param name="position">The position this shape should have.</param>
public TransformedShape(Shape shape, FPMatrix orientation, FPVector position)
{
this.position = position;
this.orientation = orientation;
FPMatrix.Transpose(ref orientation, out invOrientation);
this.shape = shape;
this.boundingBox = new FPBBox();
UpdateBoundingBox();
}
/// <summary>
/// Gets the axis aligned bounding box of the orientated shape.
/// </summary>
/// <param name="orientation">The orientation of the shape.</param>
/// <param name="box">The axis aligned bounding box of the shape.</param>
public override void GetBoundingBox(ref FPMatrix orientation, out FPBBox box)
{
FPMatrix abs; FPMath.Absolute(ref orientation, out abs);
FPVector temp;
FPVector.Transform(ref halfSize, ref abs, out temp);
box.max = temp;
FPVector.Negate(ref temp, out box.min);
}
public void UpdateBoundingBox()
{
boundingBox = FPBBox.SmallBox;
boundingBox.AddPoint(ref owner.points[indices.I0].position);
boundingBox.AddPoint(ref owner.points[indices.I1].position);
boundingBox.AddPoint(ref owner.points[indices.I2].position);
boundingBox.min -= new FPVector(owner.triangleExpansion);
boundingBox.max += new FPVector(owner.triangleExpansion);
}
/// <summary>
/// Creates a new box containing the two given ones.
/// </summary>
/// <param name="original">First box.</param>
/// <param name="additional">Second box.</param>
/// <param name="result">A JBBox containing the two given boxes.</param>
public static void CreateMerged(ref FPBBox original, ref FPBBox additional, out FPBBox result)
{
FPVector vector;
FPVector vector2;
FPVector.Min(ref original.min, ref additional.min, out vector2);
FPVector.Max(ref original.max, ref additional.max, out vector);
result.min = vector2;
result.max = vector;
}
public void Clone(RigidBody rb)
{
this.marker = rb.marker;
this.affectedByGravity = rb.affectedByGravity;
this.boundingBox = rb.boundingBox;
this.internalIndex = rb.internalIndex;
this.inverseMass = rb.inverseMass;
this.isColliderOnly = rb.isColliderOnly;
this.isStatic = rb.isStatic;
this.isKinematic = rb.isKinematic;
this.sweptDirection = rb.sweptDirection;
this.position = rb.Position;
this.orientation = rb.Orientation;
this.linearVelocity = rb.LinearVelocity;
this.angularVelocity = rb.AngularVelocity;
this.inertia = rb.Inertia;
this.invInertia = rb.InverseInertia;
this.invInertiaWorld = rb.InverseInertiaWorld;
this.invOrientation = rb.invOrientation;
this.force = rb.Force;
this.torque = rb.Torque;
this.shapeClone = poolGenericShapeClone.GetNew();
this.shapeClone.Clone(rb.Shape);
this.connections.Clear();
for (index = 0, length = rb.connections.Count; index < length; index++)
{
this.connections.Add(rb.connections[index]);
}
this.constraints.Clear();
for (index = 0, length = rb.constraints.Count; index < length; index++)
{
this.constraints.Add(rb.constraints[index]);
}
this.isActive = rb.IsActive;
this.inactiveTime = rb.inactiveTime;
this.marker = rb.marker;
this.disabled = rb.disabled;
this.freezeConstraint = rb._freezeConstraints;
this._freezePosition = rb._freezePosition;
this._freezeRotation = rb._freezeRotation;
this._freezeRotationQuat = rb._freezeRotationQuat;
this.prevKinematicGravity = rb.prevKinematicGravity;
this.linearDrag = rb.linearDrag;
this.angularDrag = rb.angularDrag;
this.staticFriction = rb.staticFriction;
this.restitution = rb.restitution;
}
public virtual void Update(FP timestep)
{
active = false;
foreach (MassPoint point in points)
{
if (point.isActive && !point.isStatic)
{
active = true; break;
}
}
if (!active)
{
return;
}
box = FPBBox.SmallBox;
volume = FP.Zero;
mass = FP.Zero;
foreach (MassPoint point in points)
{
mass += point.Mass;
box.AddPoint(point.position);
}
box.min -= new FPVector(TriangleExpansion);
box.max += new FPVector(TriangleExpansion);
foreach (Triangle t in triangles)
{
// Update bounding box and move proxy in dynamic tree.
t.UpdateBoundingBox();
FPVector linVel = t.VertexBody1.linearVelocity +
t.VertexBody2.linearVelocity +
t.VertexBody3.linearVelocity;
linVel *= FP.One / (3 * FP.One);
dynamicTree.MoveProxy(t.dynamicTreeID, ref t.boundingBox, linVel * timestep);
FPVector v1 = points[t.indices.I0].position;
FPVector v2 = points[t.indices.I1].position;
FPVector v3 = points[t.indices.I2].position;
volume -= ((v2.y - v1.y) * (v3.z - v1.z) -
(v2.z - v1.z) * (v3.y - v1.y)) * (v1.x + v2.x + v3.x);
}
volume /= (FP)6;
AddPressureForces(timestep);
}
/// <summary>
/// Passes a axis aligned bounding box to the shape where collision
/// could occour.
/// </summary>
/// <param name="box">The bounding box where collision could occur.</param>
/// <returns>The upper index with which <see cref="SetCurrentShape"/> can be
/// called.</returns>
public override int Prepare(ref FPBBox box)
{
// simple idea: the terrain is a grid. x and z is the position in the grid.
// y the height. we know compute the min and max grid-points. All quads
// between these points have to be checked.
// including overflow exception prevention
if (box.min.x < boundings.min.x)
{
minX = 0;
}
else
{
minX = (int)FP.Floor(((box.min.x - sphericalExpansion) / scaleX));
minX = (int)FPMath.Max(minX, 0);
}
if (box.max.x > boundings.max.x)
{
maxX = heightsLength0 - 1;
}
else
{
maxX = (int)FP.Ceiling(((box.max.x + sphericalExpansion) / scaleX));
maxX = (int)FPMath.Min(maxX, heightsLength0 - 1);
}
if (box.min.z < boundings.min.z)
{
minZ = 0;
}
else
{
minZ = (int)FP.Floor(((box.min.z - sphericalExpansion) / scaleZ));
minZ = (int)FPMath.Max(minZ, 0);
}
if (box.max.z > boundings.max.z)
{
maxZ = heightsLength1 - 1;
}
else
{
maxZ = (int)FP.Ceiling((FP)((box.max.z + sphericalExpansion) / scaleZ));
maxZ = (int)FPMath.Min(maxZ, heightsLength1 - 1);
}
numX = maxX - minX;
numZ = maxZ - minZ;
// since every quad contains two triangles we multiply by 2.
return(numX * numZ * 2);
}
/// <summary>
/// Checks whether another bounding box is inside, outside or intersecting
/// this box.
/// </summary>
/// <param name="box">The other bounding box to check.</param>
/// <returns>The ContainmentType of the box.</returns>
public ContainmentType Contains(ref FPBBox box)
{
ContainmentType result = ContainmentType.Disjoint;
if ((((this.max.x >= box.min.x) && (this.min.x <= box.max.x)) && ((this.max.y >= box.min.y) && (this.min.y <= box.max.y))) && ((this.max.z >= box.min.z) && (this.min.z <= box.max.z)))
{
result = ((((this.min.x <= box.min.x) && (box.max.x <= this.max.x)) && ((this.min.y <= box.min.y) && (box.max.y <= this.max.y))) && ((this.min.z <= box.min.z) && (box.max.z <= this.max.z))) ? ContainmentType.Contains : ContainmentType.Intersects;
}
return(result);
}
protected void UpdateInternalBoundingBox()
{
mInternalBBox.min = new FPVector(FP.MaxValue);
mInternalBBox.max = new FPVector(FP.MinValue);
for (int i = 0; i < shapes.Length; i++)
{
shapes[i].UpdateBoundingBox();
FPBBox.CreateMerged(ref mInternalBBox, ref shapes[i].boundingBox, out mInternalBBox);
}
}
/// <summary>
/// Gets the axis aligned bounding box of the orientated shape. This includes
/// the whole shape.
/// </summary>
/// <param name="orientation">The orientation of the shape.</param>
/// <param name="box">The axis aligned bounding box of the shape.</param>
public override void GetBoundingBox(ref FPMatrix orientation, out FPBBox box)
{
FPBBox helpBox = FPBBox.LargeBox;
int length = this.Prepare(ref helpBox);
box = FPBBox.SmallBox;
for (int i = 0; i < length; i++)
{
this.SetCurrentShape(i);
base.GetBoundingBox(ref orientation, out helpBox);
FPBBox.CreateMerged(ref box, ref helpBox, out box);
}
}
/// <summary>
/// Passes a axis aligned bounding box to the shape where collision
/// could occour.
/// </summary>
/// <param name="box">The bounding box where collision could occur.</param>
/// <returns>The upper index with which <see cref="SetCurrentShape"/> can be
/// called.</returns>
public override int Prepare(ref FPBBox box)
{
currentSubShapes.Clear();
for (int i = 0; i < shapes.Length; i++)
{
if (shapes[i].boundingBox.Contains(ref box) != FPBBox.ContainmentType.Disjoint)
{
currentSubShapes.Add(i);
}
}
return(currentSubShapes.Count);
}
/// <summary>
/// Gets the axis aligned bounding box of the orientated shape. This includes
/// the whole shape.
/// </summary>
/// <param name="orientation">The orientation of the shape.</param>
/// <param name="box">The axis aligned bounding box of the shape.</param>
public override void GetBoundingBox(ref FPMatrix orientation, out FPBBox box)
{
box = octree.rootNodeBox;
#region Expand Spherical
box.min.x -= sphericalExpansion;
box.min.y -= sphericalExpansion;
box.min.z -= sphericalExpansion;
box.max.x += sphericalExpansion;
box.max.y += sphericalExpansion;
box.max.z += sphericalExpansion;
#endregion
box.Transform(ref orientation);
}
public override void MakeHull(ref List <FPVector> triangleList, int generationThreshold)
{
FPBBox large = FPBBox.LargeBox;
List <int> indices = new List <int>();
octree.GetTrianglesIntersectingtAABox(indices, ref large);
for (int i = 0; i < indices.Count; i++)
{
triangleList.Add(octree.GetVertex(octree.GetTriangleVertexIndex(i).I0));
triangleList.Add(octree.GetVertex(octree.GetTriangleVertexIndex(i).I1));
triangleList.Add(octree.GetVertex(octree.GetTriangleVertexIndex(i).I2));
}
}
/// <summary>
///
/// </summary>
/// <param name="rayOrigin"></param>
/// <param name="rayDelta"></param>
/// <returns></returns>
public override int Prepare(ref FPVector rayOrigin, ref FPVector rayDelta)
{
FPBBox box = FPBBox.SmallBox;
#region RayEnd + Expand Spherical
FPVector rayEnd;
FPVector.Normalize(ref rayDelta, out rayEnd);
rayEnd = rayOrigin + rayDelta + rayEnd * sphericalExpansion;
#endregion
box.AddPoint(ref rayOrigin);
box.AddPoint(ref rayEnd);
return(this.Prepare(ref box));
}
/// <summary>
/// Create a bounding box appropriate for a child, based on a parents AABox
/// </summary>
/// <param name="aabb"></param>
/// <param name="child"></param>
/// <param name="result"></param>
#region private void CreateAABox(ref JBBox aabb, EChild child,out JBBox result)
private void CreateAABox(ref FPBBox aabb, EChild child, out FPBBox result)
{
FPVector dims;
FPVector.Subtract(ref aabb.max, ref aabb.min, out dims);
FPVector.Multiply(ref dims, FP.Half, out dims);
FPVector offset = FPVector.zero;
switch (child)
{
case EChild.PPP: offset = new FPVector(1, 1, 1); break;
case EChild.PPM: offset = new FPVector(1, 1, 0); break;
case EChild.PMP: offset = new FPVector(1, 0, 1); break;
case EChild.PMM: offset = new FPVector(1, 0, 0); break;
case EChild.MPP: offset = new FPVector(0, 1, 1); break;
case EChild.MPM: offset = new FPVector(0, 1, 0); break;
case EChild.MMP: offset = new FPVector(0, 0, 1); break;
case EChild.MMM: offset = new FPVector(0, 0, 0); break;
default:
System.Diagnostics.Debug.WriteLine("Octree.CreateAABox got impossible child");
break;
}
result = new FPBBox();
result.min = new FPVector(offset.x * dims.x, offset.y * dims.y, offset.z * dims.z);
FPVector.Add(ref result.min, ref aabb.min, out result.min);
FPVector.Add(ref result.min, ref dims, out result.max);
// expand it just a tiny bit just to be safe!
FP extra = FP.EN5;
FPVector temp; FPVector.Multiply(ref dims, extra, out temp);
FPVector.Subtract(ref result.min, ref temp, out result.min);
FPVector.Add(ref result.max, ref temp, out result.max);
}
/// <summary>
/// Create a proxy in the tree as a leaf node. We return the index
/// of the node instead of a pointer so that we can grow
/// the node pool.
/// /// </summary>
/// <param name="aabb">The aabb.</param>
/// <param name="userData">The user data.</param>
/// <returns>Index of the created proxy</returns>
public int AddProxy(ref FPBBox aabb, T userData)
{
int proxyId = AllocateNode();
_nodes[proxyId].MinorRandomExtension = FP.Half * settingsRndExtension;
// Fatten the aabb.
FPVector r = new FPVector(_nodes[proxyId].MinorRandomExtension);
_nodes[proxyId].AABB.min = aabb.min - r;
_nodes[proxyId].AABB.max = aabb.max + r;
_nodes[proxyId].UserData = userData;
_nodes[proxyId].LeafCount = 1;
InsertLeaf(proxyId);
return(proxyId);
}
/// <summary>
/// Passes a axis aligned bounding box to the shape where collision
/// could occour.
/// </summary>
/// <param name="box">The bounding box where collision could occur.</param>
/// <returns>The upper index with which <see cref="SetCurrentShape"/> can be
/// called.</returns>
public override int Prepare(ref FPBBox box)
{
potentialTriangles.Clear();
#region Expand Spherical
FPBBox exp = box;
exp.min.x -= sphericalExpansion;
exp.min.y -= sphericalExpansion;
exp.min.z -= sphericalExpansion;
exp.max.x += sphericalExpansion;
exp.max.y += sphericalExpansion;
exp.max.z += sphericalExpansion;
#endregion
octree.GetTrianglesIntersectingtAABox(potentialTriangles, ref exp);
return(potentialTriangles.Count);
}
/// <summary>
/// Returns all triangles which intersect the given axis aligned bounding box.
/// </summary>
/// <param name="triangles">The list to add the triangles to.</param>
/// <param name="testBox">The axis alignes bounding box.</param>
/// <returns></returns>
#region public int GetTrianglesIntersectingtAABox(List<int> triangles, ref JBBox testBox)
public int GetTrianglesIntersectingtAABox(List <int> triangles, ref FPBBox testBox)
{
if (nodes.Length == 0)
{
return(0);
}
int curStackIndex = 0;
int endStackIndex = 1;
UInt16[] nodeStack = nodeStackPool.GetNew();
nodeStack[0] = 0;
int triCount = 0;
while (curStackIndex < endStackIndex)
{
UInt16 nodeIndex = nodeStack[curStackIndex];
curStackIndex++;
if (nodes[nodeIndex].box.Contains(ref testBox) != FPBBox.ContainmentType.Disjoint)
{
for (int i = 0; i < nodes[nodeIndex].triIndices.Length; ++i)
{
if (triBoxes[nodes[nodeIndex].triIndices[i]].Contains(ref testBox) != FPBBox.ContainmentType.Disjoint)
{
triangles.Add(nodes[nodeIndex].triIndices[i]);
triCount++;
}
}
int numChildren = nodes[nodeIndex].nodeIndices.Length;
for (int i = 0; i < numChildren; ++i)
{
nodeStack[endStackIndex++] = nodes[nodeIndex].nodeIndices[i];
}
}
}
nodeStackPool.GiveBack(nodeStack);
return(triCount);
}
/// <summary>
/// Gets the axis aligned bounding box of the orientated shape. (Inlcuding all
/// 'sub' shapes)
/// </summary>
/// <param name="orientation">The orientation of the shape.</param>
/// <param name="box">The axis aligned bounding box of the shape.</param>
public override void GetBoundingBox(ref FPMatrix orientation, out FPBBox box)
{
box.min = mInternalBBox.min;
box.max = mInternalBBox.max;
FPVector localHalfExtents = FP.Half * (box.max - box.min);
FPVector localCenter = FP.Half * (box.max + box.min);
FPVector center;
FPVector.Transform(ref localCenter, ref orientation, out center);
FPMatrix abs; FPMath.Absolute(ref orientation, out abs);
FPVector temp;
FPVector.Transform(ref localHalfExtents, ref abs, out temp);
box.max = center + temp;
box.min = center - temp;
}
/// <summary>
/// Query an AABB for overlapping proxies. The callback class
/// is called for each proxy that overlaps the supplied AABB.
/// </summary>
/// <param name="callback">The callback.</param>
/// <param name="aabb">The aabb.</param>
public void Query(List <int> my, ref FPBBox aabb)
{
//Stack<int> _stack = new Stack<int>(256);
Stack <int> _stack = stackPool.GetNew();
_stack.Push(_root);
while (_stack.Count > 0)
{
int nodeId = _stack.Pop();
if (nodeId == NullNode)
{
continue;
}
DynamicTreeNode <T> node = _nodes[nodeId];
//if (JBBox.TestOverlap(ref node.AABB, ref aabb))
if (aabb.Contains(ref node.AABB) != FPBBox.ContainmentType.Disjoint)
{
if (node.IsLeaf())
{
my.Add(nodeId);
//bool proceed = callback(nodeId);
//if (proceed == false)
//{
// return;
//}
}
else
{
_stack.Push(node.Child1);
_stack.Push(node.Child2);
}
}
}
stackPool.GiveBack(_stack);
}
/// <summary>
/// Initializes a new instance of the TerrainShape class.
/// </summary>
/// <param name="heights">An array containing the heights of the terrain surface.</param>
/// <param name="scaleX">The x-scale factor. (The x-space between neighbour heights)</param>
/// <param name="scaleZ">The y-scale factor. (The y-space between neighbour heights)</param>
public TerrainShape(FP[,] heights, FP scaleX, FP scaleZ)
{
heightsLength0 = heights.GetLength(0);
heightsLength1 = heights.GetLength(1);
#region Bounding Box
boundings = FPBBox.SmallBox;
for (int i = 0; i < heightsLength0; i++)
{
for (int e = 0; e < heightsLength1; e++)
{
if (heights[i, e] > boundings.max.y)
{
boundings.max.y = heights[i, e];
}
else if (heights[i, e] < boundings.min.y)
{
boundings.min.y = heights[i, e];
}
}
}
boundings.min.x = FP.Zero;
boundings.min.z = FP.Zero;
boundings.max.x = checked (heightsLength0 * scaleX);
boundings.max.z = checked (heightsLength1 * scaleZ);
#endregion
this.heights = heights;
this.scaleX = scaleX;
this.scaleZ = scaleZ;
UpdateShape();
}
