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; }