/// <summary>
/// Gets the bounding box of the shape given a transform.
/// </summary>
/// <param name="shapeTransform">Transform to use.</param>
/// <param name="boundingBox">Bounding box of the transformed shape.</param>
public override void GetBoundingBox(ref RigidTransform shapeTransform, out BoundingBox boundingBox)
{
#if !WINDOWS
boundingBox = new BoundingBox();
#endif
Matrix3x3 o;
Matrix3x3.CreateFromQuaternion(ref shapeTransform.Orientation, out o);
//Sample the local directions from the orientation matrix, implicitly transposed.
//Notice only three directions are used. Due to box symmetry, 'left' is just -right.
var right = new Vector3(Math.Sign(o.M11) * halfWidth, Math.Sign(o.M21) * halfHeight, Math.Sign(o.M31) * halfLength);
var up = new Vector3(Math.Sign(o.M12) * halfWidth, Math.Sign(o.M22) * halfHeight, Math.Sign(o.M32) * halfLength);
var backward = new Vector3(Math.Sign(o.M13) * halfWidth, Math.Sign(o.M23) * halfHeight, Math.Sign(o.M33) * halfLength);
Matrix3x3.Transform(ref right, ref o, out right);
Matrix3x3.Transform(ref up, ref o, out up);
Matrix3x3.Transform(ref backward, ref o, out backward);
//These right/up/backward represent the extreme points in world space along the world space axes.
boundingBox.Max.X = shapeTransform.Position.X + right.X;
boundingBox.Max.Y = shapeTransform.Position.Y + up.Y;
boundingBox.Max.Z = shapeTransform.Position.Z + backward.Z;
boundingBox.Min.X = shapeTransform.Position.X - right.X;
boundingBox.Min.Y = shapeTransform.Position.Y - up.Y;
boundingBox.Min.Z = shapeTransform.Position.Z - backward.Z;
}
protected override void ConfigureCollidable(TriangleEntry entry, Fix64 dt)
{
var shape = entry.Collidable.Shape;
mesh.Shape.TriangleMesh.Data.GetTriangle(entry.Index, out shape.vA, out shape.vB, out shape.vC);
Matrix3x3 o;
Matrix3x3.CreateFromQuaternion(ref mesh.worldTransform.Orientation, out o);
Matrix3x3.Transform(ref shape.vA, ref o, out shape.vA);
Matrix3x3.Transform(ref shape.vB, ref o, out shape.vB);
Matrix3x3.Transform(ref shape.vC, ref o, out shape.vC);
Vector3 center;
Vector3.Add(ref shape.vA, ref shape.vB, out center);
Vector3.Add(ref center, ref shape.vC, out center);
Vector3.Multiply(ref center, F64.OneThird, out center);
Vector3.Subtract(ref shape.vA, ref center, out shape.vA);
Vector3.Subtract(ref shape.vB, ref center, out shape.vB);
Vector3.Subtract(ref shape.vC, ref center, out shape.vC);
Vector3.Add(ref center, ref mesh.worldTransform.Position, out center);
//The bounding box doesn't update by itself.
entry.Collidable.worldTransform.Position = center;
entry.Collidable.worldTransform.Orientation = Quaternion.Identity;
entry.Collidable.UpdateBoundingBoxInternal(dt);
}
/// <summary>
/// Modifies a contribution using a transform, position, and weight.
/// </summary>
/// <param name="transform">Transform to use to modify the contribution.</param>
/// <param name="center">Center to use to modify the contribution.</param>
/// <param name="baseContribution">Original unmodified contribution.</param>
/// <param name="weight">Weight of the contribution.</param>
/// <param name="contribution">Transformed contribution.</param>
public static void TransformContribution(ref RigidTransform transform, ref Vector3 center,
ref Matrix3x3 baseContribution, float weight, out Matrix3x3 contribution)
{
Matrix3x3 rotation;
Matrix3x3.CreateFromQuaternion(ref transform.Orientation, out rotation);
Matrix3x3 temp;
//Do angular transformed contribution first...
Matrix3x3.MultiplyTransposed(ref rotation, ref baseContribution, out temp);
Matrix3x3.Multiply(ref temp, ref rotation, out temp);
contribution = temp;
//Now add in the offset from the origin.
Vector3 offset;
Vector3.Subtract(ref transform.Position, ref center, out offset);
Matrix3x3 innerProduct;
Matrix3x3.CreateScale(offset.LengthSquared(), out innerProduct);
Matrix3x3 outerProduct;
Matrix3x3.CreateOuterProduct(ref offset, ref offset, out outerProduct);
Matrix3x3.Subtract(ref innerProduct, ref outerProduct, out temp);
Matrix3x3.Add(ref contribution, ref temp, out contribution);
Matrix3x3.Multiply(ref contribution, weight, out contribution);
}
/// <summary>
/// Gets the bounding box of the shape given a transform.
/// </summary>
/// <param name="shapeTransform">Transform to use.</param>
/// <param name="boundingBox">Bounding box of the transformed shape.</param>
public override void GetBoundingBox(ref RigidTransform shapeTransform, out BoundingBox boundingBox)
{
#if !WINDOWS
boundingBox = new BoundingBox();
#endif
Matrix3x3 o;
Matrix3x3.CreateFromQuaternion(ref shapeTransform.Orientation, out o);
//Sample the local directions from the orientation matrix, implicitly transposed.
//Notice only three directions are used. Due to box symmetry, 'left' is just -right.
var right = new Vector3(Math.Sign(o.M11) * halfWidth, Math.Sign(o.M21) * halfHeight, Math.Sign(o.M31) * halfLength);
var up = new Vector3(Math.Sign(o.M12) * halfWidth, Math.Sign(o.M22) * halfHeight, Math.Sign(o.M32) * halfLength);
var backward = new Vector3(Math.Sign(o.M13) * halfWidth, Math.Sign(o.M23) * halfHeight, Math.Sign(o.M33) * halfLength);
//Rather than transforming each axis independently (and doing three times as many operations as required), just get the 3 required values directly.
Vector3 offset;
TransformLocalExtremePoints(ref right, ref up, ref backward, ref o, out offset);
//The positive and negative vectors represent the X, Y and Z coordinates of the extreme points in world space along the world space axes.
Vector3.Add(ref shapeTransform.Position, ref offset, out boundingBox.Max);
Vector3.Subtract(ref shapeTransform.Position, ref offset, out boundingBox.Min);
}
void IPositionUpdateable.PreUpdatePosition(float dt)
{
Vector3 increment;
Vector3.Multiply(ref angularVelocity, dt * .5f, out increment);
var multiplier = new Quaternion(increment.X, increment.Y, increment.Z, 0);
Quaternion.Multiply(ref multiplier, ref orientation, out multiplier);
Quaternion.Add(ref orientation, ref multiplier, out orientation);
orientation.Normalize();
Matrix3x3.CreateFromQuaternion(ref orientation, out orientationMatrix);
//Only do the linear motion if this object doesn't obey CCD.
if (PositionUpdateMode == PositionUpdateMode.Discrete)
{
Vector3.Multiply(ref linearVelocity, dt, out increment);
Vector3.Add(ref position, ref increment, out position);
collisionInformation.UpdateWorldTransform(ref position, ref orientation);
//The position update is complete if this is a discretely updated object.
if (PositionUpdated != null)
{
PositionUpdated(this);
}
}
MathChecker.Validate(linearVelocity);
MathChecker.Validate(angularVelocity);
MathChecker.Validate(position);
MathChecker.Validate(orientation);
#if CONSERVE
MathChecker.Validate(angularMomentum);
#endif
}
/// <summary>
/// Creates an affine transform from a rigid transform.
/// </summary>
/// <param name="rigid">Rigid transform to base the affine transform on.</param>
/// <returns>Affine transform created from the rigid transform.</returns>
public static AffineTransform CreateFromRigidTransform(RigidTransform rigid)
{
AffineTransform toReturn;
toReturn.Translation = rigid.Position;
Matrix3x3.CreateFromQuaternion(ref rigid.Orientation, out toReturn.LinearTransform);
return(toReturn);
}
///<summary>
/// Updates the cached simplex with the latest run's results.
///</summary>
///<param name="simplex">Simplex to update.</param>
public void UpdateCachedSimplex(ref CachedSimplex simplex)
{
simplex.LocalSimplexA = SimplexA;
switch (State)
{
case SimplexState.Point:
Vector3.Subtract(ref SimplexB.A, ref LocalTransformB.Position, out simplex.LocalSimplexB.A);
Quaternion conjugate;
Quaternion.Conjugate(ref LocalTransformB.Orientation, out conjugate);
Quaternion.Transform(ref simplex.LocalSimplexB.A, ref conjugate, out simplex.LocalSimplexB.A);
break;
case SimplexState.Segment:
Vector3.Subtract(ref SimplexB.A, ref LocalTransformB.Position, out simplex.LocalSimplexB.A);
Vector3.Subtract(ref SimplexB.B, ref LocalTransformB.Position, out simplex.LocalSimplexB.B);
Matrix3x3 transform;
Matrix3x3.CreateFromQuaternion(ref LocalTransformB.Orientation, out transform);
Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.A, ref transform,
out simplex.LocalSimplexB.A);
Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.B, ref transform,
out simplex.LocalSimplexB.B);
break;
case SimplexState.Triangle:
Vector3.Subtract(ref SimplexB.A, ref LocalTransformB.Position, out simplex.LocalSimplexB.A);
Vector3.Subtract(ref SimplexB.B, ref LocalTransformB.Position, out simplex.LocalSimplexB.B);
Vector3.Subtract(ref SimplexB.C, ref LocalTransformB.Position, out simplex.LocalSimplexB.C);
Matrix3x3.CreateFromQuaternion(ref LocalTransformB.Orientation, out transform);
Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.A, ref transform,
out simplex.LocalSimplexB.A);
Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.B, ref transform,
out simplex.LocalSimplexB.B);
Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.C, ref transform,
out simplex.LocalSimplexB.C);
break;
case SimplexState.Tetrahedron:
Vector3.Subtract(ref SimplexB.A, ref LocalTransformB.Position, out simplex.LocalSimplexB.A);
Vector3.Subtract(ref SimplexB.B, ref LocalTransformB.Position, out simplex.LocalSimplexB.B);
Vector3.Subtract(ref SimplexB.C, ref LocalTransformB.Position, out simplex.LocalSimplexB.C);
Vector3.Subtract(ref SimplexB.D, ref LocalTransformB.Position, out simplex.LocalSimplexB.D);
Matrix3x3.CreateFromQuaternion(ref LocalTransformB.Orientation, out transform);
Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.A, ref transform,
out simplex.LocalSimplexB.A);
Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.B, ref transform,
out simplex.LocalSimplexB.B);
Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.C, ref transform,
out simplex.LocalSimplexB.C);
Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.D, ref transform,
out simplex.LocalSimplexB.D);
break;
}
simplex.State = State;
}
public static void RotateInverseInertia(ref Symmetric3x3 localInverseInertiaTensor, ref Quaternion orientation, out Symmetric3x3 rotatedInverseInertiaTensor)
{
Matrix3x3.CreateFromQuaternion(orientation, out var orientationMatrix);
//I^-1 = RT * Ilocal^-1 * R
//NOTE: If you were willing to confuse users a little bit, the local inertia could be required to be diagonal.
//This would be totally fine for all the primitive types which happen to have diagonal inertias, but for more complex shapes (convex hulls, meshes),
//there would need to be a reorientation step. That could be confusing, and it's probably not worth it.
Symmetric3x3.RotationSandwich(orientationMatrix, localInverseInertiaTensor, out rotatedInverseInertiaTensor);
}
/// <summary>
/// Updates the world inertia tensor based upon the local inertia tensor and current orientation.
/// </summary>
internal void UpdateInertiaTensor()
{
//This is separate from the position update because the orientation can change outside of our iteration loop, so this has to run first.
//Iworld^-1 = RT * Ilocal^1 * R
Matrix3x3 orientationMatrix;
Matrix3x3.CreateFromQuaternion(ref Orientation, out orientationMatrix);
Matrix3x3.MultiplyTransposed(ref orientationMatrix, ref localInertiaTensorInverse, out inertiaTensorInverse);
Matrix3x3.Multiply(ref inertiaTensorInverse, ref orientationMatrix, out inertiaTensorInverse);
}
///<summary>
/// Constructs a new affine transform.
///</summary>
///<param name="scaling">Scaling to apply in the linear transform.</param>
///<param name="orientation">Orientation to apply in the linear transform.</param>
///<param name="translation">Translation to apply.</param>
public AffineTransform(ref Vector3 scaling, ref Quaternion orientation, ref Vector3 translation)
{
//Create an SRT transform.
Matrix3x3.CreateScale(ref scaling, out LinearTransform);
Matrix3x3 rotation;
Matrix3x3.CreateFromQuaternion(ref orientation, out rotation);
Matrix3x3.Multiply(ref LinearTransform, ref rotation, out LinearTransform);
Translation = translation;
}
///<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.
Matrix3x3 o;
Matrix3x3.CreateFromQuaternion(ref shapeTransform.Orientation, out o);
GetBoundingBox(ref o, out boundingBox);
Vector3.Add(ref boundingBox.Max, ref shapeTransform.Position, out boundingBox.Max);
Vector3.Add(ref boundingBox.Min, ref shapeTransform.Position, out boundingBox.Min);
}
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);
}
}
///<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.
//Matrix3X3 o;
//Matrix3X3.CreateFromQuaternion(ref shapeTransform.Orientation, out o);
////Sample the local directions from the orientation matrix, implicitly transposed.
//Vector3 right = new Vector3(o.M11 * 100000, o.M21 * 100000, o.M31 * 100000);
//Vector3 up = new Vector3(o.M12 * 100000, o.M22 * 100000, o.M32 * 100000);
//Vector3 backward = new Vector3(o.M13 * 100000, o.M23 * 100000, o.M33 * 100000);
//Vector3 left, down, forward;
//Vector3.Negate(ref right, out left);
//Vector3.Negate(ref up, out down);
//Vector3.Negate(ref backward, out forward);
//for (int i = 0; i < extents.count; i++)
//{
// extents.Elements[i].Clamp(ref right);
// extents.Elements[i].Clamp(ref left);
// extents.Elements[i].Clamp(ref up);
// extents.Elements[i].Clamp(ref down);
// extents.Elements[i].Clamp(ref backward);
// extents.Elements[i].Clamp(ref forward);
//}
//Matrix3X3.Transform(ref right, ref o, out right);
//Matrix3X3.Transform(ref left, ref o, out left);
//Matrix3X3.Transform(ref down, ref o, out down);
//Matrix3X3.Transform(ref up, ref o, out up);
//Matrix3X3.Transform(ref forward, ref o, out forward);
//Matrix3X3.Transform(ref backward, ref o, out backward);
//boundingBox.Max.X = shapeTransform.Position.X + right.X;
//boundingBox.Max.Y = shapeTransform.Position.Y + up.Y;
//boundingBox.Max.Z = shapeTransform.Position.Z + backward.Z;
//boundingBox.Min.X = shapeTransform.Position.X + left.X;
//boundingBox.Min.Y = shapeTransform.Position.Y + down.Y;
//boundingBox.Min.Z = shapeTransform.Position.Z + forward.Z;
Matrix3x3 o;
Matrix3x3.CreateFromQuaternion(ref shapeTransform.Orientation, out o);
GetBoundingBox(ref o, out boundingBox);
boundingBox.Max.X += shapeTransform.Position.X;
boundingBox.Max.Y += shapeTransform.Position.Y;
boundingBox.Max.Z += shapeTransform.Position.Z;
boundingBox.Min.X += shapeTransform.Position.X;
boundingBox.Min.Y += shapeTransform.Position.Y;
boundingBox.Min.Z += shapeTransform.Position.Z;
}
///<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)
{
Matrix3x3 linearTransform;//Have to use temporary variable just in case b reference is transform.
Matrix3x3.CreateFromQuaternion(ref a.Orientation, out linearTransform);
Matrix3x3.Multiply(ref linearTransform, ref b.LinearTransform, out linearTransform);
System.Numerics.Vector3 translation;
Matrix3x3.Transform(ref a.Position, ref b.LinearTransform, out translation);
Vector3Ex.Add(ref translation, ref b.Translation, out transform.Translation);
transform.LinearTransform = linearTransform;
}
/// <summary>
/// Gets the bounding box of the shape given a transform.
/// </summary>
/// <param name="shapeTransform">Transform to use.</param>
/// <param name="boundingBox">Bounding box of the transformed shape.</param>
public override void GetBoundingBox(ref RigidTransform shapeTransform, out BoundingBox boundingBox)
{
#if !WINDOWS
boundingBox = new BoundingBox();
#endif
Matrix3x3 o;
Matrix3x3.CreateFromQuaternion(ref shapeTransform.Orientation, out o);
//Sample the local directions from the orientation matrix, implicitly transposed.
Vector3 right;
var direction = new Vector3(o.M11, o.M21, o.M31);
GetLocalExtremePointWithoutMargin(ref direction, out right);
Vector3 left;
direction = new Vector3(-o.M11, -o.M21, -o.M31);
GetLocalExtremePointWithoutMargin(ref direction, out left);
Vector3 up;
direction = new Vector3(o.M12, o.M22, o.M32);
GetLocalExtremePointWithoutMargin(ref direction, out up);
Vector3 down;
direction = new Vector3(-o.M12, -o.M22, -o.M32);
GetLocalExtremePointWithoutMargin(ref direction, out down);
Vector3 backward;
direction = new Vector3(o.M13, o.M23, o.M33);
GetLocalExtremePointWithoutMargin(ref direction, out backward);
Vector3 forward;
direction = new Vector3(-o.M13, -o.M23, -o.M33);
GetLocalExtremePointWithoutMargin(ref direction, out forward);
Matrix3x3.Transform(ref right, ref o, out right);
Matrix3x3.Transform(ref left, ref o, out left);
Matrix3x3.Transform(ref up, ref o, out up);
Matrix3x3.Transform(ref down, ref o, out down);
Matrix3x3.Transform(ref backward, ref o, out backward);
Matrix3x3.Transform(ref forward, ref o, out forward);
//These right/up/backward represent the extreme points in world space along the world space axes.
boundingBox.Max.X = shapeTransform.Position.X + collisionMargin + right.X;
boundingBox.Max.Y = shapeTransform.Position.Y + collisionMargin + up.Y;
boundingBox.Max.Z = shapeTransform.Position.Z + collisionMargin + backward.Z;
boundingBox.Min.X = shapeTransform.Position.X - collisionMargin + left.X;
boundingBox.Min.Y = shapeTransform.Position.Y - collisionMargin + down.Y;
boundingBox.Min.Z = shapeTransform.Position.Z - collisionMargin + forward.Z;
}
/// <summary>
/// Constructs a new demo.
/// </summary>
/// <param name="game">Game owning this demo.</param>
public InverseKinematicsTestDemo2(DemosGame game)
: base(game)
{
game.Camera.Position = new Vector3(0, 3, 5);
Box ground = new Box(new Vector3(0, -3, 0), 30, 1, 30);
Space.Add(ground);
Space.ForceUpdater.Gravity = new Vector3(0, -9.81m, 0);
var solver = new IKSolver();
solver.ActiveSet.UseAutomass = true;
//solver.AutoscaleControlImpulses = true;
//solver.AutoscaleControlMaximumForce = Fix64.MaxValue;
solver.ControlIterationCount = 20;
solver.FixerIterationCount = 0;
solver.VelocitySubiterationCount = 3;
List <Bone> bones;
List <Entity> boneEntities;
int boneCount = 10;
BuildStick(new Vector3(0, 0.5m, 0), boneCount, out bones, out boneEntities);
DragControl dragger = new DragControl {
TargetBone = bones[boneCount - 1], MaximumForce = Fix64.MaxValue
};
dragger.LinearMotor.Rigidity = 16;
dragger.LinearMotor.LocalOffset = new Vector3(0, 0.5m, 0);
dragger.LinearMotor.TargetPosition = new Vector3(10, 0, 0);
bones[0].Pinned = true;
var controls = new List <Control>();
controls.Add(dragger);
solver.Solve(controls);
var tipLocation = bones[boneCount - 1].Position + Matrix3x3.CreateFromQuaternion(bones[boneCount - 1].Orientation).Up * 0.5m;
for (int i = 0; i < bones.Count; ++i)
{
boneEntities[i].Position = bones[i].Position;
boneEntities[i].Orientation = bones[i].Orientation;
Space.Add(boneEntities[i]);
}
}
private void IntegrateCallback(object obj)
{
RigidBody body = obj as RigidBody;
Vector3.Multiply(ref body.linearVelocity, timestep, out Vector3 temp);
Vector3.Add(ref temp, ref body.position, out body.position);
if (!(body.isParticle))
{
//exponential map
Vector3 axis;
float angle = body.angularVelocity.magnitude;
if (angle < 0.001f)
{
// use Taylor's expansions of sync function
// axis = body.angularVelocity * (0.5f * timestep - (timestep * timestep * timestep) * (0.020833333333f) * angle * angle);
Vector3.Multiply(ref body.angularVelocity, (0.5f * timestep - (timestep * timestep * timestep) * (0.020833333333f) * angle * angle), out axis);
}
else
{
// sync(fAngle) = sin(c*fAngle)/t
Vector3.Multiply(ref body.angularVelocity, ((float)Math.Sin(0.5f * angle * timestep) / angle), out axis);
}
Quaternion dorn = new Quaternion(axis.x, axis.y, axis.z, (float)Math.Cos(angle * timestep * 0.5f));
Quaternion.CreateFromMatrix(ref body.orientation, out Quaternion ornA);
Quaternion.Multiply(ref dorn, ref ornA, out dorn);
dorn.Normalize(); Matrix3x3.CreateFromQuaternion(ref dorn, out body.orientation);
}
if ((body.Damping & RigidBody.DampingType.Linear) != 0)
{
Vector3.Multiply(ref body.linearVelocity, currentLinearDampFactor, out body.linearVelocity);
}
if ((body.Damping & RigidBody.DampingType.Angular) != 0)
{
Vector3.Multiply(ref body.angularVelocity, currentAngularDampFactor, out body.angularVelocity);
}
body.Update();
if (CollisionSystem.EnableSpeculativeContacts || body.EnableSpeculativeContacts)
{
body.SweptExpandBoundingBox(timestep);
}
}
/// <summary>
/// Gets the bounding box of the shape given a transform.
/// </summary>
/// <param name="shapeTransform">Transform to use.</param>
/// <param name="boundingBox">Bounding box of the transformed shape.</param>
public override void GetBoundingBox(ref RigidTransform shapeTransform, out BoundingBox boundingBox)
{
#if !WINDOWS
boundingBox = new BoundingBox();
#endif
Matrix3x3 o;
Matrix3x3.CreateFromQuaternion(ref shapeTransform.Orientation, out o);
//Sample the local directions from the orientation matrix, implicitly transposed.
Vector3 right;
var direction = new Vector3(o.M11, o.M21, o.M31);
GetLocalExtremePointWithoutMargin(ref direction, out right);
Vector3 left;
direction = new Vector3(-o.M11, -o.M21, -o.M31);
GetLocalExtremePointWithoutMargin(ref direction, out left);
Vector3 up;
direction = new Vector3(o.M12, o.M22, o.M32);
GetLocalExtremePointWithoutMargin(ref direction, out up);
Vector3 down;
direction = new Vector3(-o.M12, -o.M22, -o.M32);
GetLocalExtremePointWithoutMargin(ref direction, out down);
Vector3 backward;
direction = new Vector3(o.M13, o.M23, o.M33);
GetLocalExtremePointWithoutMargin(ref direction, out backward);
Vector3 forward;
direction = new Vector3(-o.M13, -o.M23, -o.M33);
GetLocalExtremePointWithoutMargin(ref direction, out forward);
//Rather than transforming each axis independently (and doing three times as many operations as required), just get the 6 required values directly.
Vector3 positive, negative;
TransformLocalExtremePoints(ref right, ref up, ref backward, ref o, out positive);
TransformLocalExtremePoints(ref left, ref down, ref forward, ref o, out negative);
//The positive and negative vectors represent the X, Y and Z coordinates of the extreme points in world space along the world space axes.
boundingBox.Max.X = shapeTransform.Position.X + positive.X + collisionMargin;
boundingBox.Max.Y = shapeTransform.Position.Y + positive.Y + collisionMargin;
boundingBox.Max.Z = shapeTransform.Position.Z + positive.Z + collisionMargin;
boundingBox.Min.X = shapeTransform.Position.X + negative.X - collisionMargin;
boundingBox.Min.Y = shapeTransform.Position.Y + negative.Y - collisionMargin;
boundingBox.Min.Z = shapeTransform.Position.Z + negative.Z - collisionMargin;
}
/// <summary>
/// Gets the intersection between the triangle and the ray.
/// </summary>
/// <param name="ray">Ray to test against the triangle.</param>
/// <param name="transform">Transform to apply to the triangle shape for the test.</param>
/// <param name="maximumLength">Maximum distance to travel in units of the direction vector's length.</param>
/// <param name="hit">Hit data of the ray cast, if any.</param>
/// <returns>Whether or not the ray hit the target.</returns>
public override bool RayTest(ref Ray ray, ref RigidTransform transform, Fix64 maximumLength, out RayHit hit)
{
Matrix3x3 orientation;
Matrix3x3.CreateFromQuaternion(ref transform.Orientation, out orientation);
Ray localRay;
Quaternion conjugate;
Quaternion.Conjugate(ref transform.Orientation, out conjugate);
Quaternion.Transform(ref ray.Direction, ref conjugate, out localRay.Direction);
Vector3.Subtract(ref ray.Position, ref transform.Position, out localRay.Position);
Quaternion.Transform(ref localRay.Position, ref conjugate, out localRay.Position);
bool toReturn = Toolbox.FindRayTriangleIntersection(ref localRay, maximumLength, sidedness, ref vA, ref vB, ref vC, out hit);
//Move the hit back into world space.
Vector3.Multiply(ref ray.Direction, hit.T, out hit.Location);
Vector3.Add(ref ray.Position, ref hit.Location, out hit.Location);
Quaternion.Transform(ref hit.Normal, ref transform.Orientation, out hit.Normal);
return toReturn;
}
/// <summary>
/// Recalculates the bounding box of the fluid based on its depth, surface normal, and surface triangles.
/// </summary>
public void RecalculateBoundingBox()
{
var points = CommonResources.GetVectorList();
foreach (var tri in SurfaceTriangles)
{
points.Add(tri[0]);
points.Add(tri[1]);
points.Add(tri[2]);
points.Add(tri[0] - upVector * MaxDepth);
points.Add(tri[1] - upVector * MaxDepth);
points.Add(tri[2] - upVector * MaxDepth);
}
boundingBox = BoundingBox.CreateFromPoints(points);
CommonResources.GiveBack(points);
//Compute the transforms used to pull objects into fluid local space.
QuaternionEx.GetQuaternionBetweenNormalizedVectors(ref Toolbox.UpVector, ref upVector, out surfaceTransform.Orientation);
Matrix3x3.CreateFromQuaternion(ref surfaceTransform.Orientation, out toSurfaceRotationMatrix);
surfaceTransform.Position = surfaceTriangles[0][0];
}
void IPositionUpdateable.PreUpdatePosition(float dt)
{
Vector3 increment;
if (MotionSettings.UseRk4AngularIntegration && isDynamic)
{
Toolbox.UpdateOrientationRK4(ref orientation, ref localInertiaTensorInverse, ref angularMomentum, dt, out orientation);
}
else
{
Vector3.Multiply(ref angularVelocity, dt * .5f, out increment);
var multiplier = new Quaternion(increment.X, increment.Y, increment.Z, 0);
Quaternion.Multiply(ref multiplier, ref orientation, out multiplier);
Quaternion.Add(ref orientation, ref multiplier, out orientation);
orientation.Normalize();
}
Matrix3x3.CreateFromQuaternion(ref orientation, out orientationMatrix);
//Only do the linear motion if this object doesn't obey CCD.
if (PositionUpdateMode == PositionUpdateMode.Discrete)
{
Vector3.Multiply(ref linearVelocity, dt, out increment);
Vector3.Add(ref position, ref increment, out position);
collisionInformation.UpdateWorldTransform(ref position, ref orientation);
//The position update is complete if this is a discretely updated object.
if (PositionUpdated != null)
{
PositionUpdated(this);
}
}
collisionInformation.UpdateWorldTransform(ref position, ref orientation);
MathChecker.Validate(linearMomentum);
MathChecker.Validate(linearVelocity);
MathChecker.Validate(angularMomentum);
MathChecker.Validate(angularVelocity);
MathChecker.Validate(position);
MathChecker.Validate(orientation);
}
///<summary>
/// Tests a ray against the surface of the mesh. This does not take into account solidity.
///</summary>
///<param name="ray">Ray to test.</param>
///<param name="maximumLength">Maximum length of the ray to test; in units of the ray's direction's length.</param>
///<param name="sidedness">Sidedness to use during the ray cast. This does not have to be the same as the mesh's sidedness.</param>
///<param name="rayHit">The hit location of the ray on the mesh, if any.</param>
///<returns>Whether or not the ray hit the mesh.</returns>
public bool RayCast(Ray ray, Fix64 maximumLength, TriangleSidedness sidedness, out RayHit rayHit)
{
//Put the ray into local space.
Ray localRay;
Matrix3x3 orientation;
Matrix3x3.CreateFromQuaternion(ref worldTransform.Orientation, out orientation);
Matrix3x3.TransformTranspose(ref ray.Direction, ref orientation, out localRay.Direction);
Vector3.Subtract(ref ray.Position, ref worldTransform.Position, out localRay.Position);
Matrix3x3.TransformTranspose(ref localRay.Position, ref orientation, out localRay.Position);
if (Shape.TriangleMesh.RayCast(localRay, maximumLength, sidedness, out rayHit))
{
//Transform the hit into world space.
Vector3.Multiply(ref ray.Direction, rayHit.T, out rayHit.Location);
Vector3.Add(ref rayHit.Location, ref ray.Position, out rayHit.Location);
Matrix3x3.Transform(ref rayHit.Normal, ref orientation, out rayHit.Normal);
return(true);
}
rayHit = new RayHit();
return(false);
}
public void Test(Random random, int innerIterations)
{
Quaternion q;
q.X = (float)random.NextDouble() * 2 - 1;
q.Y = (float)random.NextDouble() * 2 - 1;
q.Z = (float)random.NextDouble() * 2 - 1;
q.W = (float)random.NextDouble() * 2 - 1;
Quaternion.NormalizeRef(ref q);
for (int i = 0; i < innerIterations; ++i)
{
Matrix3x3.CreateFromQuaternion(q, out var r);
Quaternion.CreateFromRotationMatrix(r, out var qTest);
#if DEBUG
const float epsilon = 1e-6f;
var lengthX = r.X.Length();
var lengthY = r.Y.Length();
var lengthZ = r.Z.Length();
Debug.Assert(
Math.Abs(1 - lengthX) < epsilon &&
Math.Abs(1 - lengthY) < epsilon &&
Math.Abs(1 - lengthZ) < epsilon);
if (qTest.X * q.X < 0)
{
Quaternion.Negate(qTest, out qTest);
}
Debug.Assert(
Math.Abs(qTest.X - q.X) < epsilon &&
Math.Abs(qTest.Y - q.Y) < epsilon &&
Math.Abs(qTest.Z - q.Z) < epsilon &&
Math.Abs(qTest.W - q.W) < epsilon);
#endif
}
}
/// <summary>
/// Gets the bounding box of the shape given a transform.
/// </summary>
/// <param name="shapeTransform">Transform to use.</param>
/// <param name="boundingBox">Bounding box of the transformed shape.</param>
public override void GetBoundingBox(ref RigidTransform shapeTransform, out BoundingBox boundingBox)
{
Vector3 a, b, c;
Matrix3x3 o;
Matrix3x3.CreateFromQuaternion(ref shapeTransform.Orientation, out o);
Matrix3x3.Transform(ref vA, ref o, out a);
Matrix3x3.Transform(ref vB, ref o, out b);
Matrix3x3.Transform(ref vC, ref o, out c);
Vector3.Min(ref a, ref b, out boundingBox.Min);
Vector3.Min(ref c, ref boundingBox.Min, out boundingBox.Min);
Vector3.Max(ref a, ref b, out boundingBox.Max);
Vector3.Max(ref c, ref boundingBox.Max, out boundingBox.Max);
boundingBox.Min.X += shapeTransform.Position.X - collisionMargin;
boundingBox.Min.Y += shapeTransform.Position.Y - collisionMargin;
boundingBox.Min.Z += shapeTransform.Position.Z - collisionMargin;
boundingBox.Max.X += shapeTransform.Position.X + collisionMargin;
boundingBox.Max.Y += shapeTransform.Position.Y + collisionMargin;
boundingBox.Max.Z += shapeTransform.Position.Z + collisionMargin;
}
/// <summary>
/// Gets the bounding box of the shape given a transform.
/// </summary>
/// <param name="shapeTransform">Transform to use.</param>
/// <param name="boundingBox">Bounding box of the transformed shape.</param>
public override void GetBoundingBox(ref RigidTransform shapeTransform, out BoundingBox boundingBox)
{
#if !WINDOWS
boundingBox = new BoundingBox();
#endif
Matrix3x3 o;
Matrix3x3.CreateFromQuaternion(ref shapeTransform.Orientation, out o);
//Sample the local directions from the orientation matrix, implicitly transposed.
//Notice only three directions are used. Due to box symmetry, 'left' is just -right.
var direction = new Vector3(o.M11, o.M21, o.M31);
Vector3 right;
GetLocalExtremePointWithoutMargin(ref direction, out right);
direction = new Vector3(o.M12, o.M22, o.M32);
Vector3 up;
GetLocalExtremePointWithoutMargin(ref direction, out up);
direction = new Vector3(o.M13, o.M23, o.M33);
Vector3 backward;
GetLocalExtremePointWithoutMargin(ref direction, out backward);
Matrix3x3.Transform(ref right, ref o, out right);
Matrix3x3.Transform(ref up, ref o, out up);
Matrix3x3.Transform(ref backward, ref o, out backward);
//These right/up/backward represent the extreme points in world space along the world space axes.
boundingBox.Max.X = shapeTransform.Position.X + collisionMargin + right.X;
boundingBox.Max.Y = shapeTransform.Position.Y + collisionMargin + up.Y;
boundingBox.Max.Z = shapeTransform.Position.Z + collisionMargin + backward.Z;
boundingBox.Min.X = shapeTransform.Position.X - collisionMargin - right.X;
boundingBox.Min.Y = shapeTransform.Position.Y - collisionMargin - up.Y;
boundingBox.Min.Z = shapeTransform.Position.Z - collisionMargin - backward.Z;
}
/// <summary>
/// Gets the bounding box of the shape given a transform.
/// </summary>
/// <param name="shapeTransform">Transform to use.</param>
/// <param name="boundingBox">Bounding box of the transformed shape.</param>
public override void GetBoundingBox(ref RigidTransform shapeTransform, out BoundingBox boundingBox)
{
#if !WINDOWS
boundingBox = new BoundingBox();
#endif
Matrix3x3 o;
Matrix3x3.CreateFromQuaternion(ref shapeTransform.Orientation, out o);
//Sample the local directions from the orientation matrix, implicitly transposed.
//Notice only three directions are used. Due to cylinder symmetry, 'left' is just -right.
var direction = new System.Numerics.Vector3(o.M11, o.M21, o.M31);
System.Numerics.Vector3 right;
GetLocalExtremePointWithoutMargin(ref direction, out right);
direction = new System.Numerics.Vector3(o.M12, o.M22, o.M32);
System.Numerics.Vector3 up;
GetLocalExtremePointWithoutMargin(ref direction, out up);
direction = new System.Numerics.Vector3(o.M13, o.M23, o.M33);
System.Numerics.Vector3 backward;
GetLocalExtremePointWithoutMargin(ref direction, out backward);
//Rather than transforming each axis independently (and doing three times as many operations as required), just get the 3 required values directly.
System.Numerics.Vector3 positive;
TransformLocalExtremePoints(ref right, ref up, ref backward, ref o, out positive);
//The positive and negative vectors represent the X, Y and Z coordinates of the extreme points in world space along the world space axes.
boundingBox.Max.X = shapeTransform.Position.X + positive.X + collisionMargin;
boundingBox.Max.Y = shapeTransform.Position.Y + positive.Y + collisionMargin;
boundingBox.Max.Z = shapeTransform.Position.Z + positive.Z + collisionMargin;
boundingBox.Min.X = shapeTransform.Position.X - positive.X - collisionMargin;
boundingBox.Min.Y = shapeTransform.Position.Y - positive.Y - collisionMargin;
boundingBox.Min.Z = shapeTransform.Position.Z - positive.Z - collisionMargin;
}
public override void Update(float dt)
{
//First, refresh all existing contacts. This is an incremental manifold.
ContactRefresher.ContactRefresh(contacts, supplementData, ref convex.worldTransform, ref triangle.worldTransform, contactIndicesToRemove);
RemoveQueuedContacts();
//Compute the local triangle vertices.
//TODO: this could be quicker and cleaner.
localTriangleShape.collisionMargin = triangle.Shape.collisionMargin;
localTriangleShape.sidedness = triangle.Shape.sidedness;
Matrix3x3 orientation;
Matrix3x3.CreateFromQuaternion(ref triangle.worldTransform.Orientation, out orientation);
Matrix3x3.Transform(ref triangle.Shape.vA, ref orientation, out localTriangleShape.vA);
Matrix3x3.Transform(ref triangle.Shape.vB, ref orientation, out localTriangleShape.vB);
Matrix3x3.Transform(ref triangle.Shape.vC, ref orientation, out localTriangleShape.vC);
Vector3.Add(ref localTriangleShape.vA, ref triangle.worldTransform.Position, out localTriangleShape.vA);
Vector3.Add(ref localTriangleShape.vB, ref triangle.worldTransform.Position, out localTriangleShape.vB);
Vector3.Add(ref localTriangleShape.vC, ref triangle.worldTransform.Position, out localTriangleShape.vC);
Vector3.Subtract(ref localTriangleShape.vA, ref convex.worldTransform.Position, out localTriangleShape.vA);
Vector3.Subtract(ref localTriangleShape.vB, ref convex.worldTransform.Position, out localTriangleShape.vB);
Vector3.Subtract(ref localTriangleShape.vC, ref convex.worldTransform.Position, out localTriangleShape.vC);
Matrix3x3.CreateFromQuaternion(ref convex.worldTransform.Orientation, out orientation);
Matrix3x3.TransformTranspose(ref localTriangleShape.vA, ref orientation, out localTriangleShape.vA);
Matrix3x3.TransformTranspose(ref localTriangleShape.vB, ref orientation, out localTriangleShape.vB);
Matrix3x3.TransformTranspose(ref localTriangleShape.vC, ref orientation, out localTriangleShape.vC);
//Now, generate a contact between the two shapes.
ContactData contact;
TinyStructList <ContactData> contactList;
if (pairTester.GenerateContactCandidates(localTriangleShape, out contactList))
{
for (int i = 0; i < contactList.Count; i++)
{
contactList.Get(i, out contact);
//Put the contact into world space.
Matrix3x3.Transform(ref contact.Position, ref orientation, out contact.Position);
Vector3.Add(ref contact.Position, ref convex.worldTransform.Position, out contact.Position);
Matrix3x3.Transform(ref contact.Normal, ref orientation, out contact.Normal);
//Check if the contact is unique before proceeding.
if (IsContactUnique(ref contact))
{
//Check if adding the new contact would overflow the manifold.
if (contacts.Count == 4)
{
//Adding that contact would overflow the manifold. Reduce to the best subset.
bool addCandidate;
ContactReducer.ReduceContacts(contacts, ref contact, contactIndicesToRemove, out addCandidate);
RemoveQueuedContacts();
if (addCandidate)
{
Add(ref contact);
}
}
else
{
//Won't overflow the manifold, so just toss it in PROVIDED that it isn't too close to something else.
Add(ref contact);
}
}
}
}
else
{
//Clear out the contacts, it's separated.
for (int i = contacts.Count - 1; i >= 0; i--)
{
Remove(i);
}
}
}
/// <summary>
/// Gets the bounding box of the shape given a transform.
/// </summary>
/// <param name="shapeTransform">Transform to use.</param>
/// <param name="boundingBox">Bounding box of the transformed shape.</param>
public override void GetBoundingBox(ref RigidTransform shapeTransform, out BoundingBox boundingBox)
{
#if !WINDOWS
boundingBox = new BoundingBox();
#endif
Matrix3x3 o;
Matrix3x3.CreateFromQuaternion(ref shapeTransform.Orientation, out o);
float minX, maxX;
float minY, maxY;
float minZ, maxZ;
var right = new Vector3(o.M11, o.M21, o.M31);
var up = new Vector3(o.M12, o.M22, o.M32);
var backward = new Vector3(o.M13, o.M23, o.M33);
Vector3.Dot(ref vertices[0], ref right, out maxX);
minX = maxX;
Vector3.Dot(ref vertices[0], ref up, out maxY);
minY = maxY;
Vector3.Dot(ref vertices[0], ref backward, out maxZ);
minZ = maxZ;
int minXIndex = 0;
int maxXIndex = 0;
int minYIndex = 0;
int maxYIndex = 0;
int minZIndex = 0;
int maxZIndex = 0;
for (int i = 1; i < vertices.Length; ++i)
{
float dot;
Vector3.Dot(ref vertices[i], ref right, out dot);
if (dot < minX)
{
minX = dot;
minXIndex = i;
}
else if (dot > maxX)
{
maxX = dot;
maxXIndex = i;
}
Vector3.Dot(ref vertices[i], ref up, out dot);
if (dot < minY)
{
minY = dot;
minYIndex = i;
}
else if (dot > maxY)
{
maxY = dot;
maxYIndex = i;
}
Vector3.Dot(ref vertices[i], ref backward, out dot);
if (dot < minZ)
{
minZ = dot;
minZIndex = i;
}
else if (dot > maxZ)
{
maxZ = dot;
maxZIndex = i;
}
}
//Rather than transforming each axis independently (and doing three times as many operations as required), just get the 6 required values directly.
Vector3 positive, negative;
TransformLocalExtremePoints(ref vertices[maxXIndex], ref vertices[maxYIndex], ref vertices[maxZIndex], ref o, out positive);
TransformLocalExtremePoints(ref vertices[minXIndex], ref vertices[minYIndex], ref vertices[minZIndex], ref o, out negative);
//The positive and negative vectors represent the X, Y and Z coordinates of the extreme points in world space along the world space axes.
boundingBox.Max.X = shapeTransform.Position.X + positive.X + collisionMargin;
boundingBox.Max.Y = shapeTransform.Position.Y + positive.Y + collisionMargin;
boundingBox.Max.Z = shapeTransform.Position.Z + positive.Z + collisionMargin;
boundingBox.Min.X = shapeTransform.Position.X + negative.X - collisionMargin;
boundingBox.Min.Y = shapeTransform.Position.Y + negative.Y - collisionMargin;
boundingBox.Min.Z = shapeTransform.Position.Z + negative.Z - collisionMargin;
}
///<summary>
/// Constructs a new affine tranform.
///</summary>
///<param name="orientation">Orientation to use as the linear transform.</param>
///<param name="translation">Translation to use in the transform.</param>
public AffineTransform(ref Quaternion orientation, ref Vector3 translation)
{
Matrix3x3.CreateFromQuaternion(ref orientation, out LinearTransform);
Translation = translation;
}
/// <summary>
/// Creates an affine transform from a rigid transform.
/// </summary>
/// <param name="rigid">Rigid transform to base the affine transform on.</param>
/// <param name="affine">Affine transform created from the rigid transform.</param>
public static void CreateFromRigidTransform(ref RigidTransform rigid, out AffineTransform affine)
{
affine.Translation = rigid.Position;
Matrix3x3.CreateFromQuaternion(ref rigid.Orientation, out affine.LinearTransform);
}