public unsafe void ExtractLines(ref CenterDistancePrestepData 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]]; var targetDistance = GatherScatter.GetFirst(ref prestepBundle.TargetDistance); var color = new Vector3(0.2f, 0.2f, 1f) * tint; var packedColor = Helpers.PackColor(color); var backgroundColor = new Vector3(0f, 0f, 1f) * tint; //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 = poseB.Position - poseA.Position; 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 = poseA.Position + direction * targetDistance; if (length < targetDistance) { lines.AllocateUnsafely() = new LineInstance(poseA.Position, poseB.Position, packedDistanceColor, 0); lines.AllocateUnsafely() = new LineInstance(poseB.Position, targetEnd, packedErrorColor, 0); } else { lines.AllocateUnsafely() = new LineInstance(poseA.Position, targetEnd, packedDistanceColor, 0); lines.AllocateUnsafely() = new LineInstance(targetEnd, poseB.Position, 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); } }
void Append(ref Vector <int> mask, ref Simplex simplex, ref Vector3Wide vA, ref Vector3Wide v) { for (int i = 0; i < Vector <float> .Count; ++i) { if (mask[i] == 0) { ref var count = ref Unsafe.Add(ref GatherScatter.GetFirst(ref simplex.Count), i); ref var vTargetSlot = ref GatherScatter.GetOffsetInstance(ref Unsafe.Add(ref simplex.A, count), i); GatherScatter.GetFirst(ref vTargetSlot.X) = v.X[i]; GatherScatter.GetFirst(ref vTargetSlot.Y) = v.Y[i]; GatherScatter.GetFirst(ref vTargetSlot.Z) = v.Z[i]; ref var vATargetSlot = ref GatherScatter.GetOffsetInstance(ref Unsafe.Add(ref simplex.AOnA, count), i);
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); } }