///<summary>
/// Concatenates a rigid transform with another rigid transform.
///</summary>
///<param name="a">The first rigid transform.</param>
///<param name="b">The second rigid transform.</param>
///<param name="combined">Concatenated rigid transform.</param>
public static void Multiply(ref RigidTransform a, ref RigidTransform b, out RigidTransform combined)
{
Vector3f intermediate;
Vector3f.Transform(ref a.Position, ref b.Orientation, out intermediate);
Vector3f.Add(ref intermediate, ref b.Position, out combined.Position);
Quaternion.Concatenate(ref a.Orientation, ref b.Orientation, out combined.Orientation);
}
public override void SetWorldTransformPhysics(Vector3f trans, Quaternion rot, Vector3f scl)
{
base.SetWorldTransformPhysics(trans, rot, scl);
foreach (GameObject gi in children)
{
gi.SetWorldTransformPhysics(trans.Add(gi.GetLocalTranslation()), rot.Multiply(gi.GetLocalRotation()), scl.Multiply(gi.GetLocalScale()));
}
}
protected internal override void UpdateJacobiansAndVelocityBias()
{
//Transform the anchors and offsets into world space.
Vector3f offsetA, offsetB;
Vector3f.Transform(ref LocalAnchorA, ref ConnectionA.Orientation, out offsetA);
Vector3f.Transform(ref LocalAnchorB, ref ConnectionB.Orientation, out offsetB);
Vector3f anchorA, anchorB;
Vector3f.Add(ref ConnectionA.Position, ref offsetA, out anchorA);
Vector3f.Add(ref ConnectionB.Position, ref offsetB, out anchorB);
//Compute the distance.
Vector3f separation;
Vector3f.Subtract(ref anchorB, ref anchorA, out separation);
float currentDistance = separation.Length;
//Compute jacobians
Vector3f linearA;
#if !WINDOWS
linearA = new Vector3f();
#endif
if (currentDistance > MathHelper.Epsilon)
{
linearA.X = separation.X / currentDistance;
linearA.Y = separation.Y / currentDistance;
linearA.Z = separation.Z / currentDistance;
velocityBias = new Vector3f(errorCorrectionFactor * (currentDistance - distance), 0, 0);
}
else
{
velocityBias = new Vector3f();
linearA = new Vector3f();
}
Vector3f angularA, angularB;
Vector3f.Cross(ref offsetA, ref linearA, out angularA);
//linearB = -linearA, so just swap the cross product order.
Vector3f.Cross(ref linearA, ref offsetB, out angularB);
//Put all the 1x3 jacobians into a 3x3 matrix representation.
linearJacobianA = new Matrix3f {
M11 = linearA.X, M12 = linearA.Y, M13 = linearA.Z
};
linearJacobianB = new Matrix3f {
M11 = -linearA.X, M12 = -linearA.Y, M13 = -linearA.Z
};
angularJacobianA = new Matrix3f {
M11 = angularA.X, M12 = angularA.Y, M13 = angularA.Z
};
angularJacobianB = new Matrix3f {
M11 = angularB.X, M12 = angularB.Y, M13 = angularB.Z
};
}
/// <summary>
/// Casts a convex shape against the collidable.
/// </summary>
/// <param name="castShape">Shape to cast.</param>
/// <param name="startingTransform">Initial transform of the shape.</param>
/// <param name="sweep">Sweep to apply to the shape.</param>
/// <param name="hit">Hit data, if any.</param>
/// <returns>Whether or not the cast hit anything.</returns>
public override bool ConvexCast(CollisionShapes.ConvexShapes.ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3f sweep, out RayHit hit)
{
hit = new RayHit();
BoundingBox boundingBox;
castShape.GetSweptLocalBoundingBox(ref startingTransform, ref worldTransform, ref sweep, out boundingBox);
var tri = PhysicsThreadResources.GetTriangle();
var hitElements = CommonResources.GetIntList();
if (this.Shape.TriangleMesh.Tree.GetOverlaps(boundingBox, hitElements))
{
hit.T = float.MaxValue;
for (int i = 0; i < hitElements.Count; i++)
{
Shape.TriangleMesh.Data.GetTriangle(hitElements[i], out tri.vA, out tri.vB, out tri.vC);
AffineTransform.Transform(ref tri.vA, ref worldTransform, out tri.vA);
AffineTransform.Transform(ref tri.vB, ref worldTransform, out tri.vB);
AffineTransform.Transform(ref tri.vC, ref worldTransform, out tri.vC);
Vector3f center;
Vector3f.Add(ref tri.vA, ref tri.vB, out center);
Vector3f.Add(ref center, ref tri.vC, out center);
Vector3f.Multiply(ref center, 1f / 3f, out center);
Vector3f.Subtract(ref tri.vA, ref center, out tri.vA);
Vector3f.Subtract(ref tri.vB, ref center, out tri.vB);
Vector3f.Subtract(ref tri.vC, ref center, out tri.vC);
tri.MaximumRadius = tri.vA.LengthSquared;
float radius = tri.vB.LengthSquared;
if (tri.MaximumRadius < radius)
{
tri.MaximumRadius = radius;
}
radius = tri.vC.LengthSquared;
if (tri.MaximumRadius < radius)
{
tri.MaximumRadius = radius;
}
tri.MaximumRadius = (float)Math.Sqrt(tri.MaximumRadius);
tri.collisionMargin = 0;
var triangleTransform = new RigidTransform {
Orientation = Quaternion.Identity, Position = center
};
RayHit tempHit;
if (MPRToolbox.Sweep(castShape, tri, ref sweep, ref Toolbox.ZeroVector, ref startingTransform, ref triangleTransform, out tempHit) && tempHit.T < hit.T)
{
hit = tempHit;
}
}
tri.MaximumRadius = 0;
PhysicsThreadResources.GiveBack(tri);
CommonResources.GiveBack(hitElements);
return(hit.T != float.MaxValue);
}
PhysicsThreadResources.GiveBack(tri);
CommonResources.GiveBack(hitElements);
return(false);
}
/// <summary>
/// Calculates and applies corrective impulses.
/// Called automatically by space.
/// </summary>
public override float SolveIteration()
{
#if !WINDOWS
Vector3f lambda = new Vector3f();
#else
Vector3f lambda;
#endif
//Velocity along the length.
Vector3f cross;
Vector3f aVel, bVel;
Vector3f.Cross(ref connectionA.angularVelocity, ref worldOffsetA, out cross);
Vector3f.Add(ref connectionA.linearVelocity, ref cross, out aVel);
Vector3f.Cross(ref connectionB.angularVelocity, ref worldOffsetB, out cross);
Vector3f.Add(ref connectionB.linearVelocity, ref cross, out bVel);
lambda.X = aVel.X - bVel.X + biasVelocity.X - softness * accumulatedImpulse.X;
lambda.Y = aVel.Y - bVel.Y + biasVelocity.Y - softness * accumulatedImpulse.Y;
lambda.Z = aVel.Z - bVel.Z + biasVelocity.Z - softness * accumulatedImpulse.Z;
//Turn the velocity into an impulse.
Vector3f.Transform(ref lambda, ref massMatrix, out lambda);
//Accumulate the impulse
Vector3f.Add(ref accumulatedImpulse, ref lambda, out accumulatedImpulse);
//Apply the impulse
//Constraint.applyImpulse(myConnectionA, myConnectionB, ref rA, ref rB, ref impulse);
#if !WINDOWS
Vector3f linear = new Vector3f();
#else
Vector3f linear;
#endif
if (connectionA.isDynamic)
{
linear.X = -lambda.X;
linear.Y = -lambda.Y;
linear.Z = -lambda.Z;
connectionA.ApplyLinearImpulse(ref linear);
Vector3f taImpulse;
Vector3f.Cross(ref worldOffsetA, ref linear, out taImpulse);
connectionA.ApplyAngularImpulse(ref taImpulse);
}
if (connectionB.isDynamic)
{
connectionB.ApplyLinearImpulse(ref lambda);
Vector3f tbImpulse;
Vector3f.Cross(ref worldOffsetB, ref lambda, out tbImpulse);
connectionB.ApplyAngularImpulse(ref tbImpulse);
}
return(Math.Abs(lambda.X) +
Math.Abs(lambda.Y) +
Math.Abs(lambda.Z));
}
public override void UpdateCollision(float dt)
{
WasContaining = Containing;
WasTouching = Touching;
var transform = new RigidTransform {
Orientation = Quaternion.Identity
};
DetectorVolume.TriangleMesh.Tree.GetOverlaps(convex.boundingBox, overlaps);
for (int i = 0; i < overlaps.Count; i++)
{
DetectorVolume.TriangleMesh.Data.GetTriangle(overlaps.Elements[i], out triangle.vA, out triangle.vB, out triangle.vC);
Vector3f.Add(ref triangle.vA, ref triangle.vB, out transform.Position);
Vector3f.Add(ref triangle.vC, ref transform.Position, out transform.Position);
Vector3f.Multiply(ref transform.Position, 1 / 3f, out transform.Position);
Vector3f.Subtract(ref triangle.vA, ref transform.Position, out triangle.vA);
Vector3f.Subtract(ref triangle.vB, ref transform.Position, out triangle.vB);
Vector3f.Subtract(ref triangle.vC, ref transform.Position, out triangle.vC);
//If this triangle collides with the convex, we can stop immediately since we know we're touching and not containing.)))
//[MPR is used here in lieu of GJK because the MPR implementation tends to finish quicker when objects are overlapping than GJK. The GJK implementation does better on separated objects.]
if (MPRToolbox.AreShapesOverlapping(convex.Shape, triangle, ref convex.worldTransform, ref transform))
{
Touching = true;
//The convex can't be fully contained if it's still touching the surface.
Containing = false;
overlaps.Clear();
goto events;
}
}
overlaps.Clear();
//If we get here, then there was no shell intersection.
//If the convex's center point is contained by the mesh, then the convex is fully contained.
//If this is a child pair, the CheckContainment flag may be set to false. This is because the parent has
//already determined that it is not contained (another child performed the check and found that it was not contained)
//and that it is already touching somehow (either by intersection or by containment).
//so further containment tests are unnecessary.
if (CheckContainment && DetectorVolume.IsPointContained(ref convex.worldTransform.Position, overlaps))
{
Touching = true;
Containing = true;
goto events;
}
//If we get here, then there was no surface intersection and the convex's center is not contained- the volume and convex are separate!
Touching = false;
Containing = false;
events:
NotifyDetectorVolumeOfChanges();
}
/// <summary>
/// Multiplies a transform by another transform.
/// </summary>
/// <param name="a">First transform.</param>
/// <param name="b">Second transform.</param>
/// <param name="transform">Combined transform.</param>
public static void Multiply(ref AffineTransform a, ref AffineTransform b, out AffineTransform transform)
{
Matrix3f linearTransform;//Have to use temporary variable just in case a or b reference is transform.
Matrix3f.Multiply(ref a.LinearTransform, ref b.LinearTransform, out linearTransform);
Vector3f translation;
Vector3f.Transform(ref a.Translation, ref b.LinearTransform, out translation);
Vector3f.Add(ref translation, ref b.Translation, out transform.Translation);
transform.LinearTransform = linearTransform;
}
///<summary>
/// Gets the extreme point of the shape in world space in a given direction.
///</summary>
///<param name="direction">Direction to find the extreme point in.</param>
/// <param name="shapeTransform">Transform to use for the shape.</param>
///<param name="extremePoint">Extreme point on the shape.</param>
public void GetExtremePointWithoutMargin(Vector3f direction, ref RigidTransform shapeTransform, out Vector3f extremePoint)
{
Quaternion conjugate;
Quaternion.Conjugate(ref shapeTransform.Orientation, out conjugate);
Vector3f.Transform(ref direction, ref conjugate, out direction);
GetLocalExtremePointWithoutMargin(ref direction, out extremePoint);
Vector3f.Transform(ref extremePoint, ref shapeTransform.Orientation, out extremePoint);
Vector3f.Add(ref extremePoint, ref shapeTransform.Position, out extremePoint);
}
///<summary>
/// Computes the bounding box of the transformed mesh shape.
///</summary>
///<param name="shapeTransform">Transform to apply to the shape during the bounding box calculation.</param>
///<param name="boundingBox">Bounding box containing the transformed mesh shape.</param>
public override void GetBoundingBox(ref RigidTransform shapeTransform, out BoundingBox boundingBox)
{
//TODO: Could use an approximate bounding volume. Would be cheaper at runtime and use less memory, though the box would be bigger.
Matrix3f o;
Matrix3f.FromQuaternion(ref shapeTransform.Orientation, out o);
GetBoundingBox(ref o, out boundingBox);
Vector3f.Add(ref boundingBox.Max, ref shapeTransform.Position, out boundingBox.Max);
Vector3f.Add(ref boundingBox.Min, ref shapeTransform.Position, out boundingBox.Min);
}
void AddLocalContact(ref ContactData contact, ref Matrix3f orientation, ref QuickList <ContactData> candidatesToAdd)
{
//Put the contact into world space.
Vector3f.Transform(ref contact.Position, ref orientation, out contact.Position);
Vector3f.Add(ref contact.Position, ref convex.worldTransform.Position, out contact.Position);
Vector3f.Transform(ref contact.Normal, ref orientation, out contact.Normal);
//Check to see if the contact is unique before proceeding.
if (IsContactUnique(ref contact, ref candidatesToAdd))
{
candidatesToAdd.Add(ref contact);
}
}
///<summary>
/// Multiplies a rigid transform by an affine transform.
///</summary>
///<param name="a">Rigid transform.</param>
///<param name="b">Affine transform.</param>
///<param name="transform">Combined transform.</param>
public static void Multiply(ref RigidTransform a, ref AffineTransform b, out AffineTransform transform)
{
Matrix3f linearTransform;//Have to use temporary variable just in case b reference is transform.
Matrix3f.FromQuaternion(ref a.Orientation, out linearTransform);
Matrix3f.Multiply(ref linearTransform, ref b.LinearTransform, out linearTransform);
Vector3f translation;
Vector3f.Transform(ref a.Position, ref b.LinearTransform, out translation);
Vector3f.Add(ref translation, ref b.Translation, out transform.Translation);
transform.LinearTransform = linearTransform;
}
/// <summary>
/// Refreshes the contact manifold, removing any out of date contacts
/// and updating others.
/// </summary>
public static void ContactRefresh(RawList <Contact> contacts, RawValueList <ContactSupplementData> supplementData, ref RigidTransform transformA, ref RigidTransform transformB, RawList <int> toRemove)
{
//TODO: Could also refresh normals with some trickery.
//Would also need to refresh depth using new normals, and would require some extra information.
for (int k = 0; k < contacts.Count; k++)
{
contacts.Elements[k].Validate();
ContactSupplementData data = supplementData.Elements[k];
Vector3f newPosA, newPosB;
RigidTransform.Transform(ref data.LocalOffsetA, ref transformA, out newPosA);
RigidTransform.Transform(ref data.LocalOffsetB, ref transformB, out newPosB);
//ab - (ab*n)*n
//Compute the horizontal offset.
Vector3f ab;
Vector3f.Subtract(ref newPosB, ref newPosA, out ab);
float dot;
Vector3f.Dot(ref ab, ref contacts.Elements[k].Normal, out dot);
Vector3f temp;
Vector3f.Multiply(ref contacts.Elements[k].Normal, dot, out temp);
Vector3f.Subtract(ref ab, ref temp, out temp);
dot = temp.LengthSquared;
if (dot > CollisionDetectionSettings.ContactInvalidationLengthSquared)
{
toRemove.Add(k);
}
else
{
//Depth refresh:
//Find deviation ((Ra-Rb)*N) and add to base depth.
Vector3f.Dot(ref ab, ref contacts.Elements[k].Normal, out dot);
contacts.Elements[k].PenetrationDepth = data.BasePenetrationDepth - dot;
if (contacts.Elements[k].PenetrationDepth < -CollisionDetectionSettings.maximumContactDistance)
{
toRemove.Add(k);
}
else
{
//Refresh position and ra/rb.
Vector3f newPos;
Vector3f.Add(ref newPosB, ref newPosA, out newPos);
Vector3f.Multiply(ref newPos, .5f, out newPos);
contacts.Elements[k].Position = newPos;
//This is an interesting idea, but has very little effect one way or the other.
//data.BasePenetrationDepth = contacts.Elements[k].PenetrationDepth;
//RigidTransform.TransformByInverse(ref newPos, ref transformA, out data.LocalOffsetA);
//RigidTransform.TransformByInverse(ref newPos, ref transformB, out data.LocalOffsetB);
}
contacts.Elements[k].Validate();
}
}
}
///<summary>
/// Gets the extreme point of the shape in world space in a given direction with margin expansion.
///</summary>
///<param name="direction">Direction to find the extreme point in.</param>
/// <param name="shapeTransform">Transform to use for the shape.</param>
///<param name="extremePoint">Extreme point on the shape.</param>
public void GetExtremePoint(Vector3f direction, ref RigidTransform shapeTransform, out Vector3f extremePoint)
{
GetExtremePointWithoutMargin(direction, ref shapeTransform, out extremePoint);
float directionLength = direction.LengthSquared;
if (directionLength > MathHelper.Epsilon)
{
Vector3f.Multiply(ref direction, collisionMargin / (float)Math.Sqrt(directionLength), out direction);
Vector3f.Add(ref extremePoint, ref direction, out extremePoint);
}
}
protected internal override void GetContactInformation(int index, out ContactInformation info)
{
info.Contact = ContactManifold.contacts.Elements[index];
//Find the contact's normal force.
float totalNormalImpulse = 0;
info.NormalImpulse = 0;
for (int i = 0; i < contactConstraint.penetrationConstraints.Count; i++)
{
totalNormalImpulse += contactConstraint.penetrationConstraints.Elements[i].accumulatedImpulse;
if (contactConstraint.penetrationConstraints.Elements[i].contact == info.Contact)
{
info.NormalImpulse = contactConstraint.penetrationConstraints.Elements[i].accumulatedImpulse;
}
}
//Compute friction force. Since we are using central friction, this is 'faked.'
float radius;
Vector3f.Distance(ref contactConstraint.slidingFriction.manifoldCenter, ref info.Contact.Position, out radius);
if (totalNormalImpulse > 0)
{
info.FrictionImpulse = (info.NormalImpulse / totalNormalImpulse) * (contactConstraint.slidingFriction.accumulatedImpulse.Length + contactConstraint.twistFriction.accumulatedImpulse * radius);
}
else
{
info.FrictionImpulse = 0;
}
//Compute relative velocity
Vector3f velocity;
//If the pair is handling some type of query and does not actually have supporting entities, then consider the velocity contribution to be zero.
if (EntityA != null)
{
Vector3f.Subtract(ref info.Contact.Position, ref EntityA.position, out velocity);
Vector3f.Cross(ref EntityA.angularVelocity, ref velocity, out velocity);
Vector3f.Add(ref velocity, ref EntityA.linearVelocity, out info.RelativeVelocity);
}
else
{
info.RelativeVelocity = new Vector3f();
}
if (EntityB != null)
{
Vector3f.Subtract(ref info.Contact.Position, ref EntityB.position, out velocity);
Vector3f.Cross(ref EntityB.angularVelocity, ref velocity, out velocity);
Vector3f.Add(ref velocity, ref EntityB.linearVelocity, out velocity);
Vector3f.Subtract(ref info.RelativeVelocity, ref velocity, out info.RelativeVelocity);
}
info.Pair = this;
}
protected override void ComputeTangents()
{
if (ControlPoints.Count == 1)
{
tangents.Add(Vector3f.Zero);
return;
}
if (ControlPoints.Count == 2)
{
Vector3f tangent = ControlPoints[1].Value - ControlPoints[0].Value;
tangents.Add(tangent);
tangents.Add(tangent);
return;
}
Vector3f tangentA, tangentB;
Vector3f previous, current, next;
//First endpoint
current = ControlPoints[0].Value;
next = ControlPoints[1].Value;
Vector3f.Subtract(ref next, ref current, out tangentA);
Vector3f.Multiply(ref tangentA, (float)(.5 / (ControlPoints[1].Time - ControlPoints[0].Time)), out tangentA);
//Vector3f.Multiply(ref current, .5f / (controlPoints[0].time), out tangentB);
//Vector3f.Add(ref tangentA, ref tangentB, out tangentA);
tangents.Add(tangentA);
for (int i = 1; i < ControlPoints.Count - 1; i++)
{
previous = current;
current = next;
next = ControlPoints[i + 1].Value;
Vector3f.Subtract(ref next, ref current, out tangentA);
Vector3f.Subtract(ref current, ref previous, out tangentB);
Vector3f.Multiply(ref tangentA, (float)(.5 / (ControlPoints[i + 1].Time - ControlPoints[i].Time)), out tangentA);
Vector3f.Multiply(ref tangentB, (float)(.5 / (ControlPoints[i].Time - ControlPoints[i - 1].Time)), out tangentB);
Vector3f.Add(ref tangentA, ref tangentB, out tangentA);
tangents.Add(tangentA);
}
previous = current;
current = next;
Vector3f.Negate(ref current, out tangentA);
Vector3f.Subtract(ref current, ref previous, out tangentB);
int currentIndex = ControlPoints.Count - 1;
int previousIndex = currentIndex - 1;
//Vector3f.Multiply(ref tangentA, .5f / (-controlPoints[currentIndex].time), out tangentA);
Vector3f.Multiply(ref tangentB, (float)(.5 / (ControlPoints[currentIndex].Time - ControlPoints[previousIndex].Time)), out tangentB);
//Vector3f.Add(ref tangentA, ref tangentB, out tangentA);
tangents.Add(tangentB);
}
void GetBuoyancyInformation(EntityCollidable collidable, out float submergedVolume, out Vector3f submergedCenter)
{
BoundingBox entityBoundingBox;
RigidTransform localTransform;
RigidTransform.MultiplyByInverse(ref collidable.worldTransform, ref surfaceTransform, out localTransform);
collidable.Shape.GetBoundingBox(ref localTransform, out entityBoundingBox);
if (entityBoundingBox.Min.Y > 0)
{
//Fish out of the water. Don't need to do raycast tests on objects not at the boundary.
submergedVolume = 0;
submergedCenter = collidable.worldTransform.Position;
return;
}
if (entityBoundingBox.Max.Y < 0)
{
submergedVolume = collidable.entity.CollisionInformation.Shape.Volume;
submergedCenter = collidable.worldTransform.Position;
return;
}
Vector3f origin, xSpacing, zSpacing;
float perColumnArea;
GetSamplingOrigin(ref entityBoundingBox, out xSpacing, out zSpacing, out perColumnArea, out origin);
float boundingBoxHeight = entityBoundingBox.Max.Y - entityBoundingBox.Min.Y;
float maxLength = -entityBoundingBox.Min.Y;
submergedCenter = new Vector3f();
submergedVolume = 0;
for (int i = 0; i < samplePointsPerDimension; i++)
{
for (int j = 0; j < samplePointsPerDimension; j++)
{
Vector3f columnVolumeCenter;
float submergedHeight;
if ((submergedHeight = GetSubmergedHeight(collidable, maxLength, boundingBoxHeight, ref origin, ref xSpacing, ref zSpacing, i, j, out columnVolumeCenter)) > 0)
{
float columnVolume = submergedHeight * perColumnArea;
Vector3f.Multiply(ref columnVolumeCenter, columnVolume, out columnVolumeCenter);
Vector3f.Add(ref columnVolumeCenter, ref submergedCenter, out submergedCenter);
submergedVolume += columnVolume;
}
}
}
Vector3f.Divide(ref submergedCenter, submergedVolume, out submergedCenter);
//Pull the submerged center into world space before applying the force.
RigidTransform.Transform(ref submergedCenter, ref surfaceTransform, out submergedCenter);
}
///<summary>
/// Constructs a new kinematic capsule.
///</summary>
///<param name="start">Line segment start point.</param>
///<param name="end">Line segment end point.</param>
///<param name="radius">Radius of the capsule to expand the line segment by.</param>
public Capsule(Vector3f start, Vector3f end, float radius)
: this((end - start).Length, radius)
{
float length;
Quaternion orientation;
GetCapsuleInformation(ref start, ref end, out orientation, out length);
this.Orientation = orientation;
Vector3f position;
Vector3f.Add(ref start, ref end, out position);
Vector3f.Multiply(ref position, .5f, out position);
this.Position = position;
}
/// <summary>
/// Cylinder shape used to compute the expanded bounding box of the character.
/// </summary>
void ExpandBoundingBox()
{
if (Body.ActivityInformation.IsActive)
{
//This runs after the bounding box updater is run, but before the broad phase.
//Expanding the character's bounding box ensures that minor variations in velocity will not cause
//any missed information.
//TODO: seems a bit silly to do this work sequentially. Would be better if it could run in parallel in the proper location.
var down = Down;
var boundingBox = Body.CollisionInformation.BoundingBox;
//Expand the bounding box up and down using the step height.
Vector3f expansion;
Vector3f.Multiply(ref down, StepManager.MaximumStepHeight, out expansion);
expansion.X = Math.Abs(expansion.X);
expansion.Y = Math.Abs(expansion.Y);
expansion.Z = Math.Abs(expansion.Z);
//When the character climbs a step, it teleports horizontally a little to gain support. Expand the bounding box to accommodate the margin.
//Compute the expansion caused by the extra radius along each axis.
//There's a few ways to go about doing this.
//The following is heavily cooked, but it is based on the angle between the vertical axis and a particular axis.
//Given that, the amount of the radial expansion required along that axis can be computed.
//The dot product would provide the cos(angle) between the vertical axis and a chosen axis.
//Equivalently, it is how much expansion would be along that axis, if the vertical axis was the axis of expansion.
//However, it's not. The dot product actually gives us the expansion along an axis perpendicular to the chosen axis, pointing away from the character's vertical axis.
//What we need is actually given by the sin(angle), which is given by ||verticalAxis x testAxis||.
//The sin(angle) is the projected length of the verticalAxis (not the expansion!) on the axis perpendicular to the testAxis pointing away from the character's vertical axis.
//That projected length, however is equal to the expansion along the test axis, which is exactly what we want.
//To show this, try setting up the triangles at the corner of a cylinder with the world axes and cylinder axes.
//Since the test axes we're using are all standard directions ({0,0,1}, {0,1,0}, and {0,0,1}), most of the cross product logic simplifies out, and we are left with:
var horizontalExpansionAmount = Body.CollisionInformation.Shape.CollisionMargin * 1.1f;
Vector3f squaredDown;
squaredDown.X = down.X * down.X;
squaredDown.Y = down.Y * down.Y;
squaredDown.Z = down.Z * down.Z;
expansion.X += horizontalExpansionAmount * (float)Math.Sqrt(squaredDown.Y + squaredDown.Z);
expansion.Y += horizontalExpansionAmount * (float)Math.Sqrt(squaredDown.X + squaredDown.Z);
expansion.Z += horizontalExpansionAmount * (float)Math.Sqrt(squaredDown.X + squaredDown.Y);
Vector3f.Add(ref expansion, ref boundingBox.Max, out boundingBox.Max);
Vector3f.Subtract(ref boundingBox.Min, ref expansion, out boundingBox.Min);
Body.CollisionInformation.BoundingBox = boundingBox;
}
}
///<summary>
/// Gets the extreme point of the minkowski difference of shapeA and shapeB in the local space of shapeA.
///</summary>
///<param name="shapeA">First shape.</param>
///<param name="shapeB">Second shape.</param>
///<param name="direction">Extreme point direction in local space.</param>
///<param name="localTransformB">Transform of shapeB in the local space of A.</param>
///<param name="extremePoint">The extreme point in the local space of A.</param>
public static void GetLocalMinkowskiExtremePoint(ConvexShape shapeA, ConvexShape shapeB, ref Vector3f direction, ref RigidTransform localTransformB, out Vector3f extremePoint)
{
//Extreme point of A-B along D = (extreme point of A along D) - (extreme point of B along -D)
shapeA.GetLocalExtremePointWithoutMargin(ref direction, out extremePoint);
Vector3f v;
Vector3f negativeN;
Vector3f.Negate(ref direction, out negativeN);
shapeB.GetExtremePointWithoutMargin(negativeN, ref localTransformB, out v);
Vector3f.Subtract(ref extremePoint, ref v, out extremePoint);
ExpandMinkowskiSum(shapeA.collisionMargin, shapeB.collisionMargin, ref direction, out v);
Vector3f.Add(ref extremePoint, ref v, out extremePoint);
}
public void GetEntries(BoundingSphere boundingShape, IList <BroadPhaseEntry> overlaps)
{
//Create a bounding box based on the bounding sphere.
//Compute the min and max of the bounding box.
//Loop through the cells and select bounding boxes which overlap the x axis.
#if !WINDOWS
Vector3f offset = new Vector3f();
#else
Vector3f offset;
#endif
offset.X = boundingShape.Radius;
offset.Y = offset.X;
offset.Z = offset.Y;
BoundingBox box;
Vector3f.Add(ref boundingShape.Center, ref offset, out box.Max);
Vector3f.Subtract(ref boundingShape.Center, ref offset, out box.Min);
Int2 min, max;
Grid2DSortAndSweep.ComputeCell(ref box.Min, out min);
Grid2DSortAndSweep.ComputeCell(ref box.Max, out max);
for (int i = min.Y; i <= max.Y; i++)
{
for (int j = min.Z; j <= max.Z; j++)
{
//Grab the cell that we are currently in.
Int2 cellIndex;
cellIndex.Y = i;
cellIndex.Z = j;
GridCell2D cell;
if (owner.cellSet.TryGetCell(ref cellIndex, out cell))
{
//To fully accelerate this, the entries list would need to contain both min and max interval markers.
//Since it only contains the sorted min intervals, we can't just start at a point in the middle of the list.
//Consider some giant bounding box that spans the entire list.
for (int k = 0; k < cell.entries.Count &&
cell.entries.Elements[k].item.boundingBox.Min.X <= box.Max.X; k++) //TODO: Try additional x axis pruning? A bit of optimization potential due to overlap with AABB test.
{
bool intersects;
var item = cell.entries.Elements[k].item;
item.boundingBox.Intersects(ref boundingShape, out intersects);
if (intersects && !overlaps.Contains(item))
{
overlaps.Add(item);
}
}
}
}
}
}
/// <summary>
/// Calculates and applies corrective impulses.
/// Called automatically by space.
/// </summary>
public override float SolveIteration()
{
float linearSpeed = entity.linearVelocity.LengthSquared;
if (linearSpeed > maximumSpeedSquared)
{
linearSpeed = (float)Math.Sqrt(linearSpeed);
Vector3f impulse;
//divide by linearSpeed to normalize the velocity.
//Multiply by linearSpeed - maximumSpeed to get the 'velocity change vector.'
Vector3f.Multiply(ref entity.linearVelocity, -(linearSpeed - maximumSpeed) / linearSpeed, out impulse);
//incorporate softness
Vector3f softnessImpulse;
Vector3f.Multiply(ref accumulatedImpulse, usedSoftness, out softnessImpulse);
Vector3f.Subtract(ref impulse, ref softnessImpulse, out impulse);
//Transform into impulse
Vector3f.Multiply(ref impulse, effectiveMassMatrix, out impulse);
//Accumulate
Vector3f previousAccumulatedImpulse = accumulatedImpulse;
Vector3f.Add(ref accumulatedImpulse, ref impulse, out accumulatedImpulse);
float forceMagnitude = accumulatedImpulse.LengthSquared;
if (forceMagnitude > maxForceDtSquared)
{
//max / impulse gives some value 0 < x < 1. Basically, normalize the vector (divide by the length) and scale by the maximum.
float multiplier = maxForceDt / (float)Math.Sqrt(forceMagnitude);
accumulatedImpulse.X *= multiplier;
accumulatedImpulse.Y *= multiplier;
accumulatedImpulse.Z *= multiplier;
//Since the limit was exceeded by this corrective impulse, limit it so that the accumulated impulse remains constrained.
impulse.X = accumulatedImpulse.X - previousAccumulatedImpulse.X;
impulse.Y = accumulatedImpulse.Y - previousAccumulatedImpulse.Y;
impulse.Z = accumulatedImpulse.Z - previousAccumulatedImpulse.Z;
}
entity.ApplyLinearImpulse(ref impulse);
return(Math.Abs(impulse.X) + Math.Abs(impulse.Y) + Math.Abs(impulse.Z));
}
return(0);
}
///<summary>
/// Gets the extreme point of the shape in local space in a given direction.
///</summary>
///<param name="direction">Direction to find the extreme point in.</param>
///<param name="extremePoint">Extreme point on the shape.</param>
public override void GetLocalExtremePointWithoutMargin(ref Vector3f direction, out Vector3f extremePoint)
{
var transform = new RigidTransform {
Orientation = shapes.WrappedList.Elements[0].Orientation
};
shapes.WrappedList.Elements[0].CollisionShape.GetExtremePoint(direction, ref transform, out extremePoint);
for (int i = 1; i < shapes.WrappedList.Count; i++)
{
Vector3f temp;
transform.Orientation = shapes.WrappedList.Elements[i].Orientation;
shapes.WrappedList.Elements[i].CollisionShape.GetExtremePoint(direction, ref transform, out temp);
Vector3f.Add(ref extremePoint, ref temp, out extremePoint);
}
Vector3f.Add(ref extremePoint, ref localOffset, out extremePoint);
}
protected internal override void UpdateJacobiansAndVelocityBias()
{
//Transform the anchors and offsets into world space.
Vector3f offsetA, offsetB, lineDirection;
Vector3f.Transform(ref LocalPlaneAnchor, ref ConnectionA.Orientation, out offsetA);
Vector3f.Transform(ref LocalPlaneNormal, ref ConnectionA.Orientation, out lineDirection);
Vector3f.Transform(ref LocalAnchorB, ref ConnectionB.Orientation, out offsetB);
Vector3f anchorA, anchorB;
Vector3f.Add(ref ConnectionA.Position, ref offsetA, out anchorA);
Vector3f.Add(ref ConnectionB.Position, ref offsetB, out anchorB);
//Compute the distance.
Vector3f separation;
Vector3f.Subtract(ref anchorB, ref anchorA, out separation);
//This entire constraint is very similar to the IKDistanceLimit, except the current distance is along an axis.
float currentDistance;
Vector3f.Dot(ref separation, ref lineDirection, out currentDistance);
velocityBias = new Vector3f(errorCorrectionFactor * currentDistance, 0, 0);
//Compute jacobians
Vector3f angularA, angularB;
//We can't just use the offset to anchor for A's jacobian- the 'collision' location is way out there at anchorB!
Vector3f rA;
Vector3f.Subtract(ref anchorB, ref ConnectionA.Position, out rA);
Vector3f.Cross(ref rA, ref lineDirection, out angularA);
//linearB = -linearA, so just swap the cross product order.
Vector3f.Cross(ref lineDirection, ref offsetB, out angularB);
//Put all the 1x3 jacobians into a 3x3 matrix representation.
linearJacobianA = new Matrix3f {
M11 = lineDirection.X, M12 = lineDirection.Y, M13 = lineDirection.Z
};
linearJacobianB = new Matrix3f {
M11 = -lineDirection.X, M12 = -lineDirection.Y, M13 = -lineDirection.Z
};
angularJacobianA = new Matrix3f {
M11 = angularA.X, M12 = angularA.Y, M13 = angularA.Z
};
angularJacobianB = new Matrix3f {
M11 = angularB.X, M12 = angularB.Y, M13 = angularB.Z
};
}
/// <summary>
/// Computes one iteration of the constraint to meet the solver updateable's goal.
/// </summary>
/// <returns>The rough applied impulse magnitude.</returns>
public override float SolveIteration()
{
//Compute relative velocity
Vector3f lambda;
Vector3f.Cross(ref r, ref entity.angularVelocity, out lambda);
Vector3f.Subtract(ref lambda, ref entity.linearVelocity, out lambda);
//Add in bias velocity
Vector3f.Add(ref biasVelocity, ref lambda, out lambda);
//Add in softness
Vector3f softnessVelocity;
Vector3f.Multiply(ref accumulatedImpulse, usedSoftness, out softnessVelocity);
Vector3f.Subtract(ref lambda, ref softnessVelocity, out lambda);
//In terms of an impulse (an instantaneous change in momentum), what is it?
Vector3f.Transform(ref lambda, ref effectiveMassMatrix, out lambda);
//Sum the impulse.
Vector3f previousAccumulatedImpulse = accumulatedImpulse;
accumulatedImpulse += lambda;
//If the impulse it takes to get to the goal is too high for the motor to handle, scale it back.
float sumImpulseLengthSquared = accumulatedImpulse.LengthSquared;
if (sumImpulseLengthSquared > maxForceDtSquared)
{
//max / impulse gives some value 0 < x < 1. Basically, normalize the vector (divide by the length) and scale by the maximum.
accumulatedImpulse *= maxForceDt / (float)Math.Sqrt(sumImpulseLengthSquared);
//Since the limit was exceeded by this corrective impulse, limit it so that the accumulated impulse remains constrained.
lambda = accumulatedImpulse - previousAccumulatedImpulse;
}
entity.ApplyLinearImpulse(ref lambda);
Vector3f taImpulse;
Vector3f.Cross(ref r, ref lambda, out taImpulse);
entity.ApplyAngularImpulse(ref taImpulse);
return(Math.Abs(lambda.X) + Math.Abs(lambda.Y) + Math.Abs(lambda.Z));
}
///<summary>
/// Computes the expansion of the minkowski sum due to margins in a given direction.
///</summary>
///<param name="marginA">First margin.</param>
///<param name="marginB">Second margin.</param>
///<param name="direction">Extreme point direction.</param>
///<param name="toExpandA">Margin contribution to the shapeA.</param>
///<param name="toExpandB">Margin contribution to the shapeB.</param>
public static void ExpandMinkowskiSum(float marginA, float marginB, Vector3f direction, ref Vector3f toExpandA, ref Vector3f toExpandB)
{
float lengthSquared = direction.LengthSquared;
if (lengthSquared > MathHelper.Epsilon)
{
lengthSquared = 1 / (float)Math.Sqrt(lengthSquared);
//The contribution to the minkowski sum by the margin is:
//direction * marginA - (-direction) * marginB.
Vector3f contribution;
Vector3f.Multiply(ref direction, marginA * lengthSquared, out contribution);
Vector3f.Add(ref toExpandA, ref contribution, out toExpandA);
Vector3f.Multiply(ref direction, marginB * lengthSquared, out contribution);
Vector3f.Subtract(ref toExpandB, ref contribution, out toExpandB);
}
//If the direction is too small, then the expansion values are left unchanged.
}
/// <summary>
/// Computes one iteration of the constraint to meet the solver updateable's goal.
/// </summary>
/// <returns>The rough applied impulse magnitude.</returns>
public override float SolveIteration()
{
//TODO: This could technically be faster.
//Form the jacobian explicitly.
//Cross cross add add subtract dot
//vs
//dot dot dot dot and then scalar adds
Vector3f dv;
Vector3f aVel, bVel;
Vector3f.Cross(ref connectionA.angularVelocity, ref rA, out aVel);
Vector3f.Add(ref aVel, ref connectionA.linearVelocity, out aVel);
Vector3f.Cross(ref connectionB.angularVelocity, ref rB, out bVel);
Vector3f.Add(ref bVel, ref connectionB.linearVelocity, out bVel);
Vector3f.Subtract(ref aVel, ref bVel, out dv);
float velocityDifference;
Vector3f.Dot(ref dv, ref worldPlaneNormal, out velocityDifference);
//if(velocityDifference > 0)
// Debug.WriteLine("Velocity difference: " + velocityDifference);
//Debug.WriteLine("softness velocity: " + softness * accumulatedImpulse);
float lambda = negativeEffectiveMass * (velocityDifference + biasVelocity + softness * accumulatedImpulse);
accumulatedImpulse += lambda;
Vector3f impulse;
Vector3f torque;
Vector3f.Multiply(ref worldPlaneNormal, lambda, out impulse);
if (connectionA.isDynamic)
{
Vector3f.Multiply(ref rAcrossN, lambda, out torque);
connectionA.ApplyLinearImpulse(ref impulse);
connectionA.ApplyAngularImpulse(ref torque);
}
if (connectionB.isDynamic)
{
Vector3f.Negate(ref impulse, out impulse);
Vector3f.Multiply(ref rBcrossN, lambda, out torque);
connectionB.ApplyLinearImpulse(ref impulse);
connectionB.ApplyAngularImpulse(ref torque);
}
return(lambda);
}
///<summary>
/// Gets the point on the segment closest to the origin.
///</summary>
///<param name="point">Point closest to origin.</param>
public void GetPointOnSegmentClosestToOrigin(out Vector3f point)
{
Vector3f segmentDisplacement;
Vector3f.Subtract(ref B, ref A, out segmentDisplacement);
float dotA;
Vector3f.Dot(ref segmentDisplacement, ref A, out dotA);
if (dotA > 0)
{
//'Behind' segment. This can't happen in a boolean version,
//but with closest points warmstarting or raycasts, it will.
State = SimplexState.Point;
U = 1;
point = A;
return;
}
float dotB;
Vector3f.Dot(ref segmentDisplacement, ref B, out dotB);
if (dotB > 0)
{
//Inside segment.
U = dotB / segmentDisplacement.LengthSquared;
V = 1 - U;
Vector3f.Multiply(ref segmentDisplacement, V, out point);
Vector3f.Add(ref point, ref A, out point);
return;
}
//It should be possible in the warmstarted closest point calculation/raycasting to be outside B.
//It is not possible in a 'boolean' GJK, where it early outs as soon as a separating axis is found.
//Outside B.
//Remove current A; we're becoming a point.
A = B;
SimplexA.A = SimplexA.B;
SimplexB.A = SimplexB.B;
State = SimplexState.Point;
U = 1;
point = A;
}
public void Raycast(Vector3f origin, Vector3f direction, out Vector3f hitPoint)
{
Jitter.LinearMath.JVector outNormal;
RigidBody outBody;
float outFraction;
bool hit = world.CollisionSystem.Raycast(new Jitter.LinearMath.JVector(origin.x, origin.y, origin.z),
new Jitter.LinearMath.JVector(direction.x, direction.y, direction.z),
null, out outBody, out outNormal, out outFraction);
if (hit)
{
hitPoint = origin.Add(direction.Multiply(outFraction));
}
else
{
hitPoint = null;
}
}
internal bool IsPointContained(ref Vector3f point, RawList <int> triangles)
{
Vector3f rayDirection;
//Point from the approximate center of the mesh outwards.
//This is a cheap way to reduce the number of unnecessary checks when objects are external to the mesh.
Vector3f.Add(ref boundingBox.Max, ref boundingBox.Min, out rayDirection);
Vector3f.Multiply(ref rayDirection, .5f, out rayDirection);
Vector3f.Subtract(ref point, ref rayDirection, out rayDirection);
//If the point is right in the middle, we'll need a backup.
if (rayDirection.LengthSquared < .01f)
{
rayDirection = VectorHelper.Up;
}
var ray = new Ray(point, rayDirection);
triangleMesh.Tree.GetOverlaps(ray, triangles);
float minimumT = float.MaxValue;
bool minimumIsClockwise = false;
for (int i = 0; i < triangles.Count; i++)
{
Vector3f a, b, c;
triangleMesh.Data.GetTriangle(triangles.Elements[i], out a, out b, out c);
RayHit hit;
bool hitClockwise;
if (Toolbox.FindRayTriangleIntersection(ref ray, float.MaxValue, ref a, ref b, ref c, out hitClockwise, out hit))
{
if (hit.T < minimumT)
{
minimumT = hit.T;
minimumIsClockwise = hitClockwise;
}
}
}
triangles.Clear();
//If the first hit is on the inner surface, then the ray started inside the mesh.
return(minimumT < float.MaxValue && minimumIsClockwise == innerFacingIsClockwise);
}
public override void GetBoundingBox(ref RigidTransform shapeTransform, out BoundingBox boundingBox)
{
#if !WINDOWS
boundingBox = new BoundingBox();
#endif
Vector3f upExtreme;
Vector3f.TransformY(halfLength, ref shapeTransform.Orientation, out upExtreme);
if (upExtreme.X > 0)
{
boundingBox.Max.X = upExtreme.X + collisionMargin;
boundingBox.Min.X = -upExtreme.X - collisionMargin;
}
else
{
boundingBox.Max.X = -upExtreme.X + collisionMargin;
boundingBox.Min.X = upExtreme.X - collisionMargin;
}
if (upExtreme.Y > 0)
{
boundingBox.Max.Y = upExtreme.Y + collisionMargin;
boundingBox.Min.Y = -upExtreme.Y - collisionMargin;
}
else
{
boundingBox.Max.Y = -upExtreme.Y + collisionMargin;
boundingBox.Min.Y = upExtreme.Y - collisionMargin;
}
if (upExtreme.Z > 0)
{
boundingBox.Max.Z = upExtreme.Z + collisionMargin;
boundingBox.Min.Z = -upExtreme.Z - collisionMargin;
}
else
{
boundingBox.Max.Z = -upExtreme.Z + collisionMargin;
boundingBox.Min.Z = upExtreme.Z - collisionMargin;
}
Vector3f.Add(ref shapeTransform.Position, ref boundingBox.Min, out boundingBox.Min);
Vector3f.Add(ref shapeTransform.Position, ref boundingBox.Max, out boundingBox.Max);
}