public unsafe void ExtractLines(ref PointOnLineServoPrestepData prestepBundle, int setIndex, int *bodyIndices,
Bodies bodies, ref Vector3 tint, ref QuickList <LineInstance> lines)
{
//Could do bundles of constraints at a time, but eh.
var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]];
var poseB = bodies.Sets[setIndex].Poses[bodyIndices[1]];
Vector3Wide.ReadFirst(prestepBundle.LocalOffsetA, out var localOffsetA);
Vector3Wide.ReadFirst(prestepBundle.LocalOffsetB, out var localOffsetB);
Vector3Wide.ReadFirst(prestepBundle.LocalDirection, out var localDirection);
QuaternionEx.Transform(localOffsetA, poseA.Orientation, out var worldOffsetA);
QuaternionEx.Transform(localDirection, poseA.Orientation, out var worldDirection);
QuaternionEx.Transform(localOffsetB, poseB.Orientation, out var worldOffsetB);
var anchorA = poseA.Position + worldOffsetA;
var anchorB = poseB.Position + worldOffsetB;
var closestPointOnLine = Vector3.Dot(anchorB - anchorA, worldDirection) * worldDirection + anchorA;
var color = new Vector3(0.2f, 0.2f, 1f) * tint;
var packedColor = Helpers.PackColor(color);
var backgroundColor = new Vector3(0f, 0f, 1f) * tint;
lines.AllocateUnsafely() = new LineInstance(poseA.Position, anchorA, packedColor, 0);
lines.AllocateUnsafely() = new LineInstance(anchorA, closestPointOnLine, packedColor, 0);
lines.AllocateUnsafely() = new LineInstance(closestPointOnLine, anchorB, Helpers.PackColor(new Vector3(1, 0, 0) * tint), 0);
lines.AllocateUnsafely() = new LineInstance(anchorB, poseB.Position, packedColor, 0);
}
public unsafe void ExtractLines(ref BallSocketPrestepData prestepBundle, int setIndex, int *bodyIndices,
Bodies bodies, ref Vector3 tint, ref QuickList <LineInstance> lines)
{
//Could do bundles of constraints at a time, but eh.
var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]];
var poseB = bodies.Sets[setIndex].Poses[bodyIndices[1]];
Vector3Wide.ReadFirst(prestepBundle.LocalOffsetA, out var localOffsetA);
Vector3Wide.ReadFirst(prestepBundle.LocalOffsetB, out var localOffsetB);
QuaternionEx.Transform(localOffsetA, poseA.Orientation, out var worldOffsetA);
QuaternionEx.Transform(localOffsetB, poseB.Orientation, out var worldOffsetB);
var endA = poseA.Position + worldOffsetA;
var endB = poseB.Position + worldOffsetB;
var color = new Vector3(0.2f, 0.2f, 1f) * tint;
var packedColor = Helpers.PackColor(color);
var backgroundColor = new Vector3(0f, 0f, 1f) * tint;
lines.AllocateUnsafely() = new LineInstance(poseA.Position, endA, packedColor, 0);
lines.AllocateUnsafely() = new LineInstance(poseB.Position, endB, packedColor, 0);
var errorColor = new Vector3(1, 0, 0) * tint;
var packedErrorColor = Helpers.PackColor(errorColor);
lines.AllocateUnsafely() = new LineInstance(endA, endB, packedErrorColor, 0);
}
public unsafe void ExtractLines(ref LinearAxisServoPrestepData prestepBundle, int setIndex, int *bodyIndices,
Bodies bodies, ref Vector3 tint, ref QuickList <LineInstance> lines)
{
//Could do bundles of constraints at a time, but eh.
var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]];
var poseB = bodies.Sets[setIndex].Poses[bodyIndices[1]];
Vector3Wide.ReadFirst(prestepBundle.LocalOffsetA, out var localOffsetA);
Vector3Wide.ReadFirst(prestepBundle.LocalOffsetB, out var localOffsetB);
Vector3Wide.ReadFirst(prestepBundle.LocalPlaneNormal, out var localPlaneNormal);
var targetOffset = GatherScatter.GetFirst(ref prestepBundle.TargetOffset);
Matrix3x3.CreateFromQuaternion(poseA.Orientation, out var orientationA);
Matrix3x3.Transform(localOffsetA, orientationA, out var worldOffsetA);
Matrix3x3.Transform(localPlaneNormal, orientationA, out var worldPlaneNormal);
Quaternion.Transform(localOffsetB, poseB.Orientation, out var worldOffsetB);
var anchorA = poseA.Position + worldOffsetA;
var anchorB = poseB.Position + worldOffsetB;
var planeOffset = Vector3.Dot(anchorB - anchorA, worldPlaneNormal);
var closestPointOnPlane = anchorB - planeOffset * worldPlaneNormal;
var packedColor = Helpers.PackColor(new Vector3(0.2f, 0.2f, 1f) * tint);
var packedBasisColor = Helpers.PackColor(new Vector3(0.2f, 0.6f, 1f) * tint);
var backgroundColor = new Vector3(0f, 0f, 1f) * tint;
lines.AllocateUnsafely() = new LineInstance(poseA.Position, anchorA, packedColor, 0);
ContactLines.BuildOrthnormalBasis(localPlaneNormal, out var localTX, out var localTY);
Matrix3x3.Transform(localTX, orientationA, out var tX);
Matrix3x3.Transform(localTY, orientationA, out var tY);
lines.AllocateUnsafely() = new LineInstance(anchorA - tX, anchorA + tX, packedBasisColor, 0);
lines.AllocateUnsafely() = new LineInstance(anchorA - tY, anchorA + tY, packedBasisColor, 0);
lines.AllocateUnsafely() = new LineInstance(anchorA, closestPointOnPlane, packedColor, 0);
lines.AllocateUnsafely() = new LineInstance(anchorB, poseB.Position, packedColor, 0);
if (targetOffset < 0)
{
targetOffset = -targetOffset;
planeOffset = -planeOffset;
worldPlaneNormal = -worldPlaneNormal;
}
var targetPoint = closestPointOnPlane + worldPlaneNormal * targetOffset;
var packedErrorColor = Helpers.PackColor(new Vector3(1, 0, 0) * tint);
if (planeOffset > targetOffset)
{
lines.AllocateUnsafely() = new LineInstance(closestPointOnPlane, targetPoint, packedColor, 0);
lines.AllocateUnsafely() = new LineInstance(targetPoint, anchorB, packedErrorColor, 0);
}
else
{
lines.AllocateUnsafely() = new LineInstance(closestPointOnPlane, anchorB, packedColor, 0);
lines.AllocateUnsafely() = new LineInstance(anchorB, targetPoint, packedErrorColor, 0);
}
}
public unsafe void ExtractLines(ref DistanceLimitPrestepData prestepBundle, int setIndex, int *bodyIndices,
Bodies bodies, ref Vector3 tint, ref QuickList <LineInstance> lines)
{
//Could do bundles of constraints at a time, but eh.
var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]];
var poseB = bodies.Sets[setIndex].Poses[bodyIndices[1]];
Vector3Wide.ReadFirst(prestepBundle.LocalOffsetA, out var localOffsetA);
Vector3Wide.ReadFirst(prestepBundle.LocalOffsetB, out var localOffsetB);
var minimumDistance = GatherScatter.GetFirst(ref prestepBundle.MinimumDistance);
var maximumDistance = GatherScatter.GetFirst(ref prestepBundle.MaximumDistance);
QuaternionEx.Transform(localOffsetA, poseA.Orientation, out var worldOffsetA);
QuaternionEx.Transform(localOffsetB, poseB.Orientation, out var worldOffsetB);
var endA = poseA.Position + worldOffsetA;
var endB = poseB.Position + worldOffsetB;
var color = new Vector3(0.2f, 0.2f, 1f) * tint;
var packedColor = Helpers.PackColor(color);
var backgroundColor = new Vector3(0f, 0f, 1f) * tint;
lines.AllocateUnsafely() = new LineInstance(poseA.Position, endA, packedColor, 0);
lines.AllocateUnsafely() = new LineInstance(poseB.Position, endB, packedColor, 0);
var offset = endB - endA;
var length = offset.Length();
var direction = length < 1e-9f ? new Vector3(1, 0, 0) : offset / length;
var errorColor = new Vector3(1, 0, 0) * tint;
var packedErrorColor = Helpers.PackColor(errorColor);
var packedFarColor = Helpers.PackColor(color * 0.5f);
var packedNearColor = Helpers.PackColor(color * 0.25f);
var minimumPoint = endA + direction * minimumDistance;
if (length >= minimumDistance && length <= maximumDistance)
{
//Create a darker bar to signify the minimum limit.
lines.AllocateUnsafely() = new LineInstance(endA, minimumPoint, packedNearColor, 0);
lines.AllocateUnsafely() = new LineInstance(minimumPoint, endB, packedFarColor, 0);
lines.AllocateUnsafely() = new LineInstance(new Vector3(float.MinValue), new Vector3(float.MinValue), 0, 0);
}
else if (length < minimumDistance)
{
//Too close; draw an error line extending beyond the connecting line.
lines.AllocateUnsafely() = new LineInstance(endA, endB, packedNearColor, 0);
lines.AllocateUnsafely() = new LineInstance(endB, endA + direction * minimumDistance, packedErrorColor, 0);
lines.AllocateUnsafely() = new LineInstance(new Vector3(float.MinValue), new Vector3(float.MinValue), 0, 0);
}
else
{
//Too far; draw an error line that extends from the desired endpoint to the current endpoint.
var targetEnd = endA + direction * maximumDistance;
lines.AllocateUnsafely() = new LineInstance(endA, minimumPoint, packedNearColor, 0);
lines.AllocateUnsafely() = new LineInstance(minimumPoint, targetEnd, packedFarColor, 0);
lines.AllocateUnsafely() = new LineInstance(targetEnd, endB, packedErrorColor, 0);
}
}
//The callbacks distinguish between convex and nonconvex because the underlying data is different.
//Nonconvex manifolds can have different normals at each contact, while convex manifolds share one normal across all contacts.
//This also affects the accumulated impulses- nonconvex impulses need to include per-contact tangent friction impulses due to the potentially differing normals.
//In all of the following, you might notice an unusual pattern- things like prestep.GetNormal(ref prestep...).
//This is a bit of a hack to work around the fact that
//1) struct member functions cannot return a reference to the 'this' instance, and
//2) there is no way in the current version of C# for an interface to require a static function.
//It's another weird little tradeoff for minimal overhead.
void ExtractConvexData <TPrestep, TAccumulatedImpulses>(ref ConstraintContacts constraintContacts, ref TPrestep prestep, ref TAccumulatedImpulses impulses)
where TPrestep : struct, IConvexContactPrestep <TPrestep>
where TAccumulatedImpulses : struct, IConvexContactAccumulatedImpulses <TAccumulatedImpulses>
{
//Note that we narrow from the raw vectorized reference into a more application-convenient AOS representation.
Vector3Wide.ReadFirst(prestep.GetNormal(ref prestep), out var normal);
//We'll approximate the per-contact friction by allocating the shared friction impulses to contacts weighted by their penetration impulse.
float totalPenetrationImpulse = 0;
for (int i = 0; i < prestep.ContactCount; ++i)
{
ref var sourceContact = ref prestep.GetContact(ref prestep, i);
ref var targetContact = ref constraintContacts.Contacts.AllocateUnsafely();
public unsafe void ExtractLines(ref OneBodyLinearServoPrestepData prestepBundle, int setIndex, int *bodyIndices,
Bodies bodies, ref Vector3 tint, ref QuickList <LineInstance> lines)
{
//Could do bundles of constraints at a time, but eh.
var pose = bodies.Sets[setIndex].Poses[*bodyIndices];
Vector3Wide.ReadFirst(prestepBundle.LocalOffset, out var localOffset);
Vector3Wide.ReadFirst(prestepBundle.Target, out var target);
Quaternion.Transform(localOffset, pose.Orientation, out var worldOffset);
var anchor = pose.Position + worldOffset;
var backgroundColor = new Vector3(0f, 0f, 1f) * tint;
lines.AllocateUnsafely() = new LineInstance(pose.Position, anchor, Helpers.PackColor(new Vector3(0.2f, 0.2f, 1f) * tint), 0);
lines.AllocateUnsafely() = new LineInstance(anchor, target, Helpers.PackColor(new Vector3(1, 0, 0) * tint), 0);
}
public unsafe void ExtractLines(ref DistanceServoPrestepData prestepBundle, int setIndex, int *bodyIndices,
Bodies bodies, ref Vector3 tint, ref QuickList <LineInstance> lines)
{
//Could do bundles of constraints at a time, but eh.
var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]];
var poseB = bodies.Sets[setIndex].Poses[bodyIndices[1]];
Vector3Wide.ReadFirst(prestepBundle.LocalOffsetA, out var localOffsetA);
Vector3Wide.ReadFirst(prestepBundle.LocalOffsetB, out var localOffsetB);
var targetDistance = GatherScatter.GetFirst(ref prestepBundle.TargetDistance);
Quaternion.Transform(localOffsetA, poseA.Orientation, out var worldOffsetA);
Quaternion.Transform(localOffsetB, poseB.Orientation, out var worldOffsetB);
var endA = poseA.Position + worldOffsetA;
var endB = poseB.Position + worldOffsetB;
var color = new Vector3(0.2f, 0.2f, 1f) * tint;
var packedColor = Helpers.PackColor(color);
var backgroundColor = new Vector3(0f, 0f, 1f) * tint;
lines.AllocateUnsafely() = new LineInstance(poseA.Position, endA, packedColor, 0);
lines.AllocateUnsafely() = new LineInstance(poseB.Position, endB, packedColor, 0);
//Draw a line from A to B. If the true distance is longer than the target distance, draw a red line to complete the gap.
//If the true distance is shorter than the target distance, draw an overshooting red line.
var offset = endB - endA;
var length = offset.Length();
var direction = length < 1e-9f ? new Vector3(1, 0, 0) : offset / length;
var errorColor = new Vector3(1, 0, 0) * tint;
var packedErrorColor = Helpers.PackColor(errorColor);
var packedDistanceColor = Helpers.PackColor(color * 0.5f);
var targetEnd = endA + direction * targetDistance;
if (length < targetDistance)
{
lines.AllocateUnsafely() = new LineInstance(endA, endB, packedDistanceColor, 0);
lines.AllocateUnsafely() = new LineInstance(endB, targetEnd, packedErrorColor, 0);
}
else
{
lines.AllocateUnsafely() = new LineInstance(endA, targetEnd, packedDistanceColor, 0);
lines.AllocateUnsafely() = new LineInstance(targetEnd, endB, packedErrorColor, 0);
}
}