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);
Quaternion.Transform(localOffsetA, poseA.Orientation, out var worldOffsetA);
Quaternion.Transform(localDirection, poseA.Orientation, out var worldDirection);
Quaternion.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 innerIndex, int setIndex, int *bodyIndices,
Bodies bodies, ref Vector3 tint, ref QuickList <LineInstance, Array <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.ReadSlot(ref prestepBundle.LocalOffsetA, innerIndex, out var localOffsetA);
Vector3Wide.ReadSlot(ref prestepBundle.LocalOffsetB, innerIndex, out var localOffsetB);
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;
var lineA = new LineInstance(poseA.Position, endA, packedColor, 0);
var lineB = new LineInstance(poseB.Position, endB, packedColor, 0);
lines.AddUnsafely(ref lineA);
lines.AddUnsafely(ref lineB);
var errorColor = new Vector3(1, 0, 0) * tint;
var packedErrorColor = Helpers.PackColor(errorColor);
var errorLine = new LineInstance(endA, endB, packedErrorColor, 0);
lines.AddUnsafely(ref errorLine);
}
public void Test(Random random, int innerIterations)
{
GenerateRandomNormalizedVector(random, out var v1);
GenerateRandomNormalizedVector(random, out var v2);
for (int i = 0; i < innerIterations; ++i)
{
Quaternion.GetQuaternionBetweenNormalizedVectors(v1, v2, out var v1ToV2);
Quaternion.GetQuaternionBetweenNormalizedVectors(v2, v1, out var v2ToV1);
#if DEBUG
Quaternion.ConcatenateWithoutOverlap(v1ToV2, v2ToV1, out var concatenated);
Quaternion.Transform(v1, v1ToV2, out var v1TransformedToV2);
Quaternion.Transform(v2, v2ToV1, out var v2TransformedToV1);
Quaternion.Transform(v1, concatenated, out var v1TransformedToV1);
var v1ToV2ErrorLength = (v1TransformedToV2 - v2).LengthSquared();
var v2ToV1ErrorLength = (v2TransformedToV1 - v1).LengthSquared();
var v1ToV1ErrorLength = (v1TransformedToV1 - v1).LengthSquared();
const float epsilon = 1e-6f;
Debug.Assert(
v1ToV2ErrorLength < epsilon &&
v2ToV1ErrorLength < epsilon &&
v1ToV1ErrorLength < epsilon);
#endif
}
}
///<summary>
/// Transforms a position by a rigid transform.
///</summary>
///<param name="position">Position to transform.</param>
///<param name="transform">Transform to apply.</param>
///<param name="result">Transformed position.</param>
public static void Transform(ref Vector3 position, ref RigidTransform transform, out Vector3 result)
{
Vector3 intermediate;
Quaternion.Transform(ref position, ref transform.Orientation, out intermediate);
result = intermediate + transform.Position;
}
///<summary>
/// Concatenates a rigid transform with another rigid transform. Assumes input and output do not overlap.
///</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 MultiplyWithoutOverlap(ref RigidTransform a, ref RigidTransform b, out RigidTransform combined)
{
Vector3 intermediate;
Quaternion.Transform(ref a.Position, ref b.Orientation, out intermediate);
combined.Position = intermediate + b.Position;
Quaternion.ConcatenateWithoutOverlap(ref a.Orientation, ref b.Orientation, out combined.Orientation);
}
///<summary>
/// Transforms a position by a rigid transform's inverse.
///</summary>
///<param name="position">Position to transform.</param>
///<param name="transform">Transform to invert and apply.</param>
///<param name="result">Transformed position.</param>
public static void TransformByInverse(ref Vector3 position, ref RigidTransform transform, out Vector3 result)
{
Quaternion orientation;
Vector3 intermediate = position - transform.Position;
Quaternion.Conjugate(ref transform.Orientation, out orientation);
Quaternion.Transform(ref intermediate, ref orientation, out result);
}
static void CreateBallSocket(ref RigidPose a, ref RigidPose b, out BallSocket description)
{
var midpoint = 0.5f * (a.Position + b.Position);
description.LocalOffsetA = Quaternion.Transform(midpoint - a.Position, Quaternion.Conjugate(a.Orientation));
description.LocalOffsetB = Quaternion.Transform(midpoint - b.Position, Quaternion.Conjugate(b.Orientation));
description.SpringSettings = new SpringSettings(15, 0.1f);
}
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);
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);
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);
}
}
static void CreateBallSocket(ref RigidPose a, ref RigidPose b, out BallSocket description)
{
var midpoint = 0.5f * (a.Position + b.Position);
description.LocalOffsetA = Quaternion.Transform(midpoint - a.Position, Quaternion.Conjugate(a.Orientation));
description.LocalOffsetB = Quaternion.Transform(midpoint - b.Position, Quaternion.Conjugate(b.Orientation));
description.SpringSettings = new SpringSettings
{
NaturalFrequency = (float)(Math.PI * 0.5f * 60),
DampingRatio = 0.1f
};
}
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 BallSocketMotorPrestepData 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.LocalOffsetB, out var localOffsetB);
Quaternion.Transform(localOffsetB, poseB.Orientation, out var worldOffsetB);
var anchor = 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, anchor, packedColor, 0);
lines.AllocateUnsafely() = new LineInstance(poseB.Position, anchor, packedColor, 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);
}
}
public unsafe void ExtractLines(ref WeldPrestepData prestepBundle, int innerIndex, 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.ReadSlot(ref prestepBundle.LocalOffset, innerIndex, out var localOffset);
Quaternion.Transform(localOffset, poseA.Orientation, out var worldOffset);
var bTarget = poseA.Position + worldOffset;
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, bTarget, packedColor, 0);
var errorColor = new Vector3(1, 0, 0) * tint;
var packedErrorColor = Helpers.PackColor(errorColor);
lines.AllocateUnsafely() = new LineInstance(bTarget, poseB.Position, packedErrorColor, 0);
}
/// <summary>
/// Inverts a rigid transform.
/// </summary>
/// <param name="transform">Transform to invert.</param>
/// <param name="inverse">Inverse of the transform.</param>
public static void Invert(ref RigidTransform transform, out RigidTransform inverse)
{
Quaternion.Conjugate(ref transform.Orientation, out inverse.Orientation);
Quaternion.Transform(ref transform.Position, ref inverse.Orientation, out inverse.Position);
inverse.Position = -inverse.Position;
}
