/// <summary>
/// Rotates the view direction up or down relative to the locked up vector.
/// </summary>
/// <param name="radians">Amount to rotate.</param>
public void Pitch(float radians)
{
//Do not allow the new view direction to violate the maximum pitch.
float dot;
Vector3.Dot(ref viewDirection, ref lockedUp, out dot);
//While this could be rephrased in terms of dot products alone, converting to actual angles can be more intuitive.
//Consider +Pi/2 to be up, and -Pi/2 to be down.
float currentPitch = (float)Math.Acos(MathHelper.Clamp(-dot, -1, 1)) - MathHelper.PiOver2;
//Compute our new pitch by clamping the current + change.
float newPitch = MathHelper.Clamp(currentPitch + radians, -maximumPitch, maximumPitch);
float allowedChange = newPitch - currentPitch;
//Compute and apply the rotation.
Vector3 pitchAxis;
Vector3.Cross(ref viewDirection, ref lockedUp, out pitchAxis);
//This is guaranteed safe by all interaction points stopping viewDirection from being aligned with lockedUp.
pitchAxis.Normalize();
Matrix3x3 rotation;
Matrix3x3.CreateFromAxisAngle(ref pitchAxis, allowedChange, out rotation);
Matrix3x3.Transform(ref viewDirection, ref rotation, out viewDirection);
//Avoid drift by renormalizing.
viewDirection.Normalize();
}
/// <summary>
/// Constructs a new demo.
/// </summary>
/// <param name="game">Game owning this demo.</param>
public AddRemoveStressDemo(DemosGame game)
: base(game)
{
Space.Remove(vehicle.Vehicle);
var compoundShape = new CompoundShape(new List <CompoundShapeEntry>
{
new CompoundShapeEntry(new BoxShape(1, 1, 1), new Vector3(0, 1, 0), 1),
new CompoundShapeEntry(new BoxShape(2, 1, 2), new Vector3(), 1),
new CompoundShapeEntry(new BoxShape(1, 1, 1), new Vector3(0, -1, 0), 1)
});
for (int i = 0; i < 300; ++i)
{
var toAdd = new Entity(compoundShape, 10);
addedEntities.Add(toAdd);
}
var boxShape = new BoxShape(1, 1, 1);
for (int i = 0; i < 300; ++i)
{
var toAdd = new Entity(boxShape, 10);
addedEntities.Add(toAdd);
}
Vector3[] vertices;
int[] indices;
ModelDataExtractor.GetVerticesAndIndicesFromModel(game.Content.Load <Model>("cube"), out vertices, out indices);
var mobileMeshShape = new MobileMeshShape(vertices, indices, new AffineTransform(Matrix3x3.CreateScale(1, 2, 1), new Vector3()), MobileMeshSolidity.Counterclockwise);
for (int i = 0; i < 300; ++i)
{
var toAdd = new Entity(mobileMeshShape, 10);
addedEntities.Add(toAdd);
}
for (int i = 0; i < addedEntities.Count; ++i)
{
var entity = addedEntities[i];
entity.Gravity = new Vector3();
entity.Position = GetRandomPosition(random);
entity.LinearVelocity = 3 * Vector3.Normalize(entity.Position);
Space.Add(entity);
}
var playgroundModel = game.Content.Load <Model>("playground");
ModelDataExtractor.GetVerticesAndIndicesFromModel(playgroundModel, out vertices, out indices);
var staticMesh = new StaticMesh(vertices, indices, new AffineTransform(Matrix3x3.CreateFromAxisAngle(Vector3.Up, MathHelper.Pi), new Vector3(0, -30, 0)));
staticMesh.Sidedness = TriangleSidedness.Counterclockwise;
Space.Add(staticMesh);
game.ModelDrawer.Add(staticMesh);
game.Camera.Position = new Vector3(0, 6, 15);
}
/// <summary>
/// Rotates the camera around its locked up vector.
/// </summary>
/// <param name="radians">Amount to rotate.</param>
public void Yaw(float radians)
{
//Rotate around the up vector.
Matrix3x3 rotation;
Matrix3x3.CreateFromAxisAngle(ref lockedUp, radians, out rotation);
Matrix3x3.Transform(ref viewDirection, ref rotation, out viewDirection);
//Avoid drift by renormalizing.
viewDirection.Normalize();
}
// Use this for initialization
public override void XStart()
{
if (!isInit)
{
meshfilter = transform.GetComponent <MeshFilter>();
position = meshfilter.transform.position;
rotation = meshfilter.transform.rotation;
mesh = meshfilter.sharedMesh;
if (transform.GetComponent <Rigidbody>() != null)
{
rigid = transform.GetComponent <Rigidbody>();
mass = rigid.mass;
}
List <BEPUutilities.Vector3> vertices = new List <BEPUutilities.Vector3>();
int[] indices = new int[mesh.GetIndexCount(0)];
indices = mesh.GetIndices(0);
for (int i = 0; i < mesh.vertices.Length; i++)
{
vertices.Add(mesh.vertices[i].ToBEPU());
}
entity = new BEPUphysics.Entities.Prefabs.MobileMesh(vertices.ToArray(), indices,
new AffineTransform(Matrix3x3.CreateFromAxisAngle(BEPUutilities.Vector3.Up, MathHelper.Pi),
new BEPUutilities.Vector3(0, -10, 0)), BEPUphysics.CollisionShapes.MobileMeshSolidity.Solid);
entity.Orientation = rotation.ToBEPU();
entity.LinearDamping = damping;
entity.AngularDamping = angdamping;
PhysicsManagerBehavior.Space.Add(entity);
this.enabled = false;
isInit = true;
}
}
public override void Update(Fix64 dt)
{
if (Game.KeyboardInput.IsKeyDown(Keys.Left))
{
rayCastDirection = Matrix3x3.Transform(rayCastDirection, Matrix3x3.CreateFromAxisAngle(Vector3.Forward, .01m));
}
if (Game.KeyboardInput.IsKeyDown(Keys.Right))
{
rayCastDirection = Matrix3x3.Transform(rayCastDirection, Matrix3x3.CreateFromAxisAngle(Vector3.Forward, -.01m));
}
if (Game.KeyboardInput.IsKeyDown(Keys.Down))
{
rayCastDirection = Matrix3x3.Transform(rayCastDirection, Matrix3x3.CreateFromAxisAngle(Vector3.Right, .01m));
}
if (Game.KeyboardInput.IsKeyDown(Keys.Up))
{
rayCastDirection = Matrix3x3.Transform(rayCastDirection, Matrix3x3.CreateFromAxisAngle(Vector3.Right, -.01m));
}
if (Game.KeyboardInput.IsKeyDown(Keys.P))
{
Debug.WriteLine("Break.");
}
base.Update(dt);
RigidTransform localTransformB;
RigidTransform aTransform = a.CollisionInformation.WorldTransform, bTransform = b.CollisionInformation.WorldTransform;
MinkowskiToolbox.GetLocalTransform(ref aTransform, ref bTransform, out localTransformB);
Vector3 position;
if (MPRToolbox.GetLocalOverlapPosition((a.CollisionInformation.Shape as ConvexShape), (b.CollisionInformation.Shape as ConvexShape), ref localTransformB, out position))
{
//Vector3 rayCastDirection = new Vector3(1,0,0);// (Vector3.Normalize(localDirection) + Vector3.Normalize(collidableB.worldTransform.Position - collidableA.worldTransform.Position)) / 2;
Fix64 previousT;
Vector3 previousNormal;
Fix64 t;
Vector3 normal;
rayCastDirection = localTransformB.Position;
MPRToolbox.LocalSurfaceCast((a.CollisionInformation.Shape as ConvexShape), (b.CollisionInformation.Shape as ConvexShape), ref localTransformB, ref rayCastDirection, out previousT, out previousNormal);
//Vector3 secondDirection = Vector3.Cross(rayCastDirection, Vector3.Up);
//MPRTesting.LocalSurfaceCast((a.CollisionInformation.Shape as ConvexShape), (b.CollisionInformation.Shape as ConvexShape), ref localTransformB, ref secondDirection, out t, out normal);
//if (t < previousT)
//{
// previousNormal = normal;
// previousT = t;
//}
//Vector3 thirdDirection = Vector3.Cross(secondDirection, rayCastDirection);
//MPRTesting.LocalSurfaceCast((a.CollisionInformation.Shape as ConvexShape), (b.CollisionInformation.Shape as ConvexShape), ref localTransformB, ref thirdDirection, out t, out normal);
//if (t < previousT)
//{
// previousNormal = normal;
// previousT = t;
//}
//Vector3 fourthDirection = -secondDirection;
//MPRTesting.LocalSurfaceCast((a.CollisionInformation.Shape as ConvexShape), (b.CollisionInformation.Shape as ConvexShape), ref localTransformB, ref fourthDirection, out t, out normal);
//if (t < previousT)
//{
// previousNormal = normal;
// previousT = t;
//}
//Vector3 fifthDirection = -thirdDirection;
//MPRTesting.LocalSurfaceCast((a.CollisionInformation.Shape as ConvexShape), (b.CollisionInformation.Shape as ConvexShape), ref localTransformB, ref fifthDirection, out t, out normal);
//if (t < previousT)
//{
// previousNormal = normal;
// previousT = t;
//}
//Correct the penetration depth.
MPRToolbox.LocalSurfaceCast((a.CollisionInformation.Shape as ConvexShape), (b.CollisionInformation.Shape as ConvexShape), ref localTransformB, ref previousNormal, out t, out normal);
contactDepth = t;
contactNormal = previousNormal;
////Converge to local minimum.
//while (true)
//{
// MPRTesting.LocalSurfaceCast((a.CollisionInformation.Shape as ConvexShape), (b.CollisionInformation.Shape as ConvexShape), ref localTransformB, ref previousNormal, out t, out normal);
// if (previousT - t <= Toolbox.BigEpsilon)
// break;
// previousT = t;
// previousNormal = normal;
//}
}
#region Box Box minkowski sum
////Construct explicit minkowski sum.
//Vector3[] aLines = new Vector3[8];
//aLines[0] = new Vector3(-boxWidth / 2, -boxHeight / 2, -boxLength / 2);
//aLines[1] = new Vector3(-boxWidth / 2, -boxHeight / 2, boxLength / 2);
//aLines[2] = new Vector3(-boxWidth / 2, boxHeight / 2, -boxLength / 2);
//aLines[3] = new Vector3(-boxWidth / 2, boxHeight / 2, boxLength / 2);
//aLines[4] = new Vector3(boxWidth / 2, -boxHeight / 2, -boxLength / 2);
//aLines[5] = new Vector3(boxWidth / 2, -boxHeight / 2, boxLength / 2);
//aLines[6] = new Vector3(boxWidth / 2, boxHeight / 2, -boxLength / 2);
//aLines[7] = new Vector3(boxWidth / 2, boxHeight / 2, boxLength / 2);
//Vector3[] bLines = new Vector3[8];
//bLines[0] = new Vector3(-groundWidth / 2, -groundHeight / 2, -groundLength / 2);
//bLines[1] = new Vector3(-groundWidth / 2, -groundHeight / 2, groundLength / 2);
//bLines[2] = new Vector3(-groundWidth / 2, groundHeight / 2, -groundLength / 2);
//bLines[3] = new Vector3(-groundWidth / 2, groundHeight / 2, groundLength / 2);
//bLines[4] = new Vector3(groundWidth / 2, -groundHeight / 2, -groundLength / 2);
//bLines[5] = new Vector3(groundWidth / 2, -groundHeight / 2, groundLength / 2);
//bLines[6] = new Vector3(groundWidth / 2, groundHeight / 2, -groundLength / 2);
//bLines[7] = new Vector3(groundWidth / 2, groundHeight / 2, groundLength / 2);
//for (int i = 0; i < 8; i++)
// aLines[i] = Vector3.Transform(aLines[i], localTransformB.Matrix);
//List<Vector3> vertices = new List<Vector3>();
//for (int i = 0; i < 8; i++)
//{
// for (int j = 0; j < 8; j++)
// {
// if (b.CollisionInformation.Pairs.Count > 0)
// {
// if (b.CollisionInformation.Pairs[0].BroadPhaseOverlap.EntryA == b.CollisionInformation)
// vertices.Add(aLines[i] - bLines[j]);
// else
// vertices.Add(bLines[i] - aLines[j]);
// }
// else
// {
// vertices.Add(bLines[i] - aLines[j]);
// }
// }
//}
//var indices = new List<int>();
//Toolbox.GetConvexHull(vertices, indices);
#endregion
#region Arbitrary minkowski sum
var vertices = new List <Vector3>();
Vector3 max;
var direction = new Vector3();
int NumSamples = 16;
Fix64 angleChange = MathHelper.TwoPi / NumSamples;
for (int i = 1; i < NumSamples / 2 - 1; i++)
{
Fix64 phi = MathHelper.PiOver2 - i * angleChange;
var sinPhi = Fix64.Sin(phi);
var cosPhi = Fix64.Cos(phi);
for (int j = 0; j < NumSamples; j++)
{
Fix64 theta = j * angleChange;
direction.X = Fix64.Cos(theta) * cosPhi;
direction.Y = sinPhi;
direction.Z = Fix64.Sin(theta) * cosPhi;
MinkowskiToolbox.GetLocalMinkowskiExtremePoint(a.CollisionInformation.Shape as ConvexShape, b.CollisionInformation.Shape as ConvexShape, ref direction, ref localTransformB, out max);
vertices.Add(max);
}
}
MinkowskiToolbox.GetLocalMinkowskiExtremePoint(a.CollisionInformation.Shape as ConvexShape, b.CollisionInformation.Shape as ConvexShape, ref Toolbox.UpVector, ref localTransformB, out max);
vertices.Add(max);
MinkowskiToolbox.GetLocalMinkowskiExtremePoint(a.CollisionInformation.Shape as ConvexShape, b.CollisionInformation.Shape as ConvexShape, ref Toolbox.DownVector, ref localTransformB, out max);
vertices.Add(max);
var indices = new List <int>();
ConvexHullHelper.GetConvexHull(vertices, indices);
#endregion
minkowskiLines.Clear();
for (int i = 0; i < indices.Count; i += 3)
{
minkowskiLines.Add(new VertexPositionColor(MathConverter.Convert(vertices[indices[i]]), Microsoft.Xna.Framework.Color.Blue));
minkowskiLines.Add(new VertexPositionColor(MathConverter.Convert(vertices[indices[i + 1]]), Microsoft.Xna.Framework.Color.Blue));
minkowskiLines.Add(new VertexPositionColor(MathConverter.Convert(vertices[indices[i + 1]]), Microsoft.Xna.Framework.Color.Blue));
minkowskiLines.Add(new VertexPositionColor(MathConverter.Convert(vertices[indices[i + 2]]), Microsoft.Xna.Framework.Color.Blue));
minkowskiLines.Add(new VertexPositionColor(MathConverter.Convert(vertices[indices[i + 2]]), Microsoft.Xna.Framework.Color.Blue));
minkowskiLines.Add(new VertexPositionColor(MathConverter.Convert(vertices[indices[i]]), Microsoft.Xna.Framework.Color.Blue));
}
}
///<summary>
/// Performs the frame's configuration step.
///</summary>
///<param name="dt">Timestep duration.</param>
public override void Update(float dt)
{
//Transform the axes into world space.
basis.rotationMatrix4 = connectionA.orientationMatrix4;
basis.ComputeWorldSpaceAxes();
Matrix3x3.Transform(ref localTestAxis, ref connectionB.orientationMatrix4, out worldTestAxis);
//Compute the plane normals.
Vector3 minPlaneNormal, maxPlaneNormal;
//Rotate basisA y axis around the basisA primary axis.
Matrix3x3 rotation;
Matrix3x3.CreateFromAxisAngle(ref basis.primaryAxis, minimumAngle + MathHelper.PiOver2, out rotation);
Matrix3x3.Transform(ref basis.xAxis, ref rotation, out minPlaneNormal);
Matrix3x3.CreateFromAxisAngle(ref basis.primaryAxis, maximumAngle - MathHelper.PiOver2, out rotation);
Matrix3x3.Transform(ref basis.xAxis, ref rotation, out maxPlaneNormal);
//Compute the errors along the two normals.
float planePositionMin, planePositionMax;
Vector3.Dot(ref minPlaneNormal, ref worldTestAxis, out planePositionMin);
Vector3.Dot(ref maxPlaneNormal, ref worldTestAxis, out planePositionMax);
float span = GetDistanceFromMinimum(maximumAngle);
//Early out and compute the determine the plane normal.
if (span >= MathHelper.Pi)
{
if (planePositionMax > 0 || planePositionMin > 0)
{
//It's in a perfectly valid configuration, so skip.
isActiveInSolver = false;
minIsActive = false;
maxIsActive = false;
error = Vector2.Zero;
accumulatedImpulse = Vector2.Zero;
isLimitActive = false;
return;
}
if (planePositionMax > planePositionMin)
{
//It's quicker to escape out to the max plane than the min plane.
error.X = 0;
error.Y = -planePositionMax;
accumulatedImpulse.X = 0;
minIsActive = false;
maxIsActive = true;
}
else
{
//It's quicker to escape out to the min plane than the max plane.
error.X = -planePositionMin;
error.Y = 0;
accumulatedImpulse.Y = 0;
minIsActive = true;
maxIsActive = false;
}
//There's never a non-degenerate situation where having both planes active with a span
//greater than pi is useful.
}
else
{
if (planePositionMax > 0 && planePositionMin > 0)
{
//It's in a perfectly valid configuration, so skip.
isActiveInSolver = false;
minIsActive = false;
maxIsActive = false;
error = Vector2.Zero;
accumulatedImpulse = Vector2.Zero;
isLimitActive = false;
return;
}
if (planePositionMin <= 0 && planePositionMax <= 0)
{
//Escape upward.
//Activate both planes.
error.X = -planePositionMin;
error.Y = -planePositionMax;
minIsActive = true;
maxIsActive = true;
}
else if (planePositionMin <= 0)
{
//It's quicker to escape out to the min plane than the max plane.
error.X = -planePositionMin;
error.Y = 0;
accumulatedImpulse.Y = 0;
minIsActive = true;
maxIsActive = false;
}
else
{
//It's quicker to escape out to the max plane than the min plane.
error.X = 0;
error.Y = -planePositionMax;
accumulatedImpulse.X = 0;
minIsActive = false;
maxIsActive = true;
}
}
isLimitActive = true;
//****** VELOCITY BIAS ******//
//Compute the correction velocity
float errorReduction;
springSettings.ComputeErrorReductionAndSoftness(dt, 1 / dt, out errorReduction, out softness);
//Compute the jacobians
if (minIsActive)
{
Vector3.Cross(ref minPlaneNormal, ref worldTestAxis, out jacobianMinA);
if (jacobianMinA.LengthSquared < Toolbox.Epsilon)
{
//The plane normal is aligned with the test axis.
//Use the basis's free axis.
jacobianMinA = basis.primaryAxis;
}
jacobianMinA.Normalize();
jacobianMinB.X = -jacobianMinA.X;
jacobianMinB.Y = -jacobianMinA.Y;
jacobianMinB.Z = -jacobianMinA.Z;
}
if (maxIsActive)
{
Vector3.Cross(ref maxPlaneNormal, ref worldTestAxis, out jacobianMaxA);
if (jacobianMaxA.LengthSquared < Toolbox.Epsilon)
{
//The plane normal is aligned with the test axis.
//Use the basis's free axis.
jacobianMaxA = basis.primaryAxis;
}
jacobianMaxA.Normalize();
jacobianMaxB.X = -jacobianMaxA.X;
jacobianMaxB.Y = -jacobianMaxA.Y;
jacobianMaxB.Z = -jacobianMaxA.Z;
}
//Error is always positive
if (minIsActive)
{
biasVelocity.X = MathHelper.Min(MathHelper.Max(0, error.X - margin) * errorReduction, maxCorrectiveVelocity);
if (bounciness > 0)
{
float relativeVelocity;
float dot;
//Find the velocity contribution from each connection
Vector3.Dot(ref connectionA.angularVelocity, ref jacobianMinA, out relativeVelocity);
Vector3.Dot(ref connectionB.angularVelocity, ref jacobianMinB, out dot);
relativeVelocity += dot;
biasVelocity.X = MathHelper.Max(biasVelocity.X, ComputeBounceVelocity(-relativeVelocity));
}
}
if (maxIsActive)
{
biasVelocity.Y = MathHelper.Min(MathHelper.Max(0, error.Y - margin) * errorReduction, maxCorrectiveVelocity);
if (bounciness > 0)
{
//Find the velocity contribution from each connection
if (maxIsActive)
{
float relativeVelocity;
Vector3.Dot(ref connectionA.angularVelocity, ref jacobianMaxA, out relativeVelocity);
float dot;
Vector3.Dot(ref connectionB.angularVelocity, ref jacobianMaxB, out dot);
relativeVelocity += dot;
biasVelocity.Y = MathHelper.Max(biasVelocity.Y, ComputeBounceVelocity(-relativeVelocity));
}
}
}
//****** EFFECTIVE MASS Matrix4 ******//
//Connection A's contribution to the mass Matrix4
float minEntryA, minEntryB;
float maxEntryA, maxEntryB;
Vector3 transformedAxis;
if (connectionA.isDynamic)
{
if (minIsActive)
{
Matrix3x3.Transform(ref jacobianMinA, ref connectionA.inertiaTensorInverse, out transformedAxis);
Vector3.Dot(ref transformedAxis, ref jacobianMinA, out minEntryA);
}
else
{
minEntryA = 0;
}
if (maxIsActive)
{
Matrix3x3.Transform(ref jacobianMaxA, ref connectionA.inertiaTensorInverse, out transformedAxis);
Vector3.Dot(ref transformedAxis, ref jacobianMaxA, out maxEntryA);
}
else
{
maxEntryA = 0;
}
}
else
{
minEntryA = 0;
maxEntryA = 0;
}
//Connection B's contribution to the mass Matrix4
if (connectionB.isDynamic)
{
if (minIsActive)
{
Matrix3x3.Transform(ref jacobianMinB, ref connectionB.inertiaTensorInverse, out transformedAxis);
Vector3.Dot(ref transformedAxis, ref jacobianMinB, out minEntryB);
}
else
{
minEntryB = 0;
}
if (maxIsActive)
{
Matrix3x3.Transform(ref jacobianMaxB, ref connectionB.inertiaTensorInverse, out transformedAxis);
Vector3.Dot(ref transformedAxis, ref jacobianMaxB, out maxEntryB);
}
else
{
maxEntryB = 0;
}
}
else
{
minEntryB = 0;
maxEntryB = 0;
}
//Compute the inverse mass Matrix4
//Notice that the mass Matrix4 isn't linked, it's two separate ones.
velocityToImpulse.X = 1 / (softness + minEntryA + minEntryB);
velocityToImpulse.Y = 1 / (softness + maxEntryA + maxEntryB);
}
/// <summary>
/// Constructs a new demo.
/// </summary>
/// <param name="game">Game owning this demo.</param>
public StaticMeshDemo(DemosGame game)
: base(game)
{
//Load in mesh data and create the collision mesh.
Vector3[] staticTriangleVertices;
int[] staticTriangleIndices;
var playgroundModel = game.Content.Load <Model>("playground");
//This is a little convenience method used to extract vertices and indices from a model.
//It doesn't do anything special; any approach that gets valid vertices and indices will work.
ModelDataExtractor.GetVerticesAndIndicesFromModel(playgroundModel, out staticTriangleVertices, out staticTriangleIndices);
var staticMesh = new StaticMesh(staticTriangleVertices, staticTriangleIndices, new AffineTransform(Matrix3x3.CreateFromAxisAngle(Vector3.Up, MathHelper.Pi), new Vector3(0, -10, 0)));
staticMesh.Sidedness = TriangleSidedness.Counterclockwise;
Space.Add(staticMesh);
game.ModelDrawer.Add(staticMesh);
//Dump some boxes on top of it for fun.
int numColumns = 8;
int numRows = 8;
int numHigh = 1;
Fix64 separation = 8;
for (int i = 0; i < numRows; i++)
{
for (int j = 0; j < numColumns; j++)
{
for (int k = 0; k < numHigh; k++)
{
var toAdd = new Box(
new Vector3(
separation * i - numRows * separation / 2,
30 + k * separation,
separation * j - numColumns * separation / 2),
2, 2, 2, 15);
Space.Add(toAdd);
}
}
}
game.Camera.Position = new Vector3(0, 10, 40);
}
/// <summary>
/// Constructs a new demo.
/// </summary>
/// <param name="game">Game owning this demo.</param>
public DogbotDemo(DemosGame game)
: base(game)
{
Entity body = new Box(new Vector3(0, 0, 0), 4, 2, 2, 20);
Space.Add(body);
Entity head = new Cone(body.Position + new Vector3(3.2f, .3f, 0), 1.5f, .7f, 4);
head.OrientationMatrix = Matrix3x3.CreateFromAxisAngle(Vector3.Forward, MathHelper.PiOver2);
Space.Add(head);
//Attach the head to the body
var universalJoint = new UniversalJoint(body, head, head.Position + new Vector3(-.8f, 0, 0));
Space.Add(universalJoint);
//Keep the head from swinging around too much.
var angleLimit = new SwingLimit(body, head, Vector3.Right, Vector3.Right, MathHelper.PiOver4);
Space.Add(angleLimit);
var tail = new Box(body.Position + new Vector3(-3f, 1f, 0), 1.6f, .1f, .1f, 4);
Space.Add(tail);
//Keep the tail from twisting itself off.
universalJoint = new UniversalJoint(body, tail, tail.Position + new Vector3(.8f, 0, 0));
Space.Add(universalJoint);
//Give 'em some floppy ears.
var ear = new Box(head.Position + new Vector3(-.2f, 0, -.65f), .01f, .7f, .2f, 1);
Space.Add(ear);
var ballSocketJoint = new BallSocketJoint(head, ear, head.Position + new Vector3(-.2f, .35f, -.65f));
Space.Add(ballSocketJoint);
ear = new Box(head.Position + new Vector3(-.2f, 0, .65f), .01f, .7f, .3f, 1);
Space.Add(ear);
ballSocketJoint = new BallSocketJoint(head, ear, head.Position + new Vector3(-.2f, .35f, .65f));
Space.Add(ballSocketJoint);
Box arm;
Cylinder shoulder;
PointOnLineJoint pointOnLineJoint;
//************* First Arm *************//
arm = new Box(body.Position + new Vector3(-1.8f, -.5f, 1.5f), .5f, 3, .2f, 20);
Space.Add(arm);
shoulder = new Cylinder(body.Position + new Vector3(-1.8f, .3f, 1.25f), .1f, .7f, 10);
shoulder.OrientationMatrix = Matrix3x3.CreateFromAxisAngle(Vector3.Right, MathHelper.PiOver2);
Space.Add(shoulder);
//Connect the shoulder to the body.
var axisJoint = new RevoluteJoint(body, shoulder, shoulder.Position, Vector3.Forward);
//Motorize the connection.
axisJoint.Motor.IsActive = true;
axisJoint.Motor.Settings.VelocityMotor.GoalVelocity = 1;
Space.Add(axisJoint);
//Connect the arm to the shoulder.
axisJoint = new RevoluteJoint(shoulder, arm, shoulder.Position + new Vector3(0, .6f, 0), Vector3.Forward);
Space.Add(axisJoint);
//Connect the arm to the body.
pointOnLineJoint = new PointOnLineJoint(arm, body, arm.Position, Vector3.Up, arm.Position + new Vector3(0, -.4f, 0));
Space.Add(pointOnLineJoint);
shoulder.OrientationMatrix *= Matrix3x3.CreateFromAxisAngle(Vector3.Forward, MathHelper.Pi); //Force the walker's legs out of phase.
//************* Second Arm *************//
arm = new Box(body.Position + new Vector3(1.8f, -.5f, 1.5f), .5f, 3, .2f, 20);
Space.Add(arm);
shoulder = new Cylinder(body.Position + new Vector3(1.8f, .3f, 1.25f), .1f, .7f, 10);
shoulder.OrientationMatrix = Matrix3x3.CreateFromAxisAngle(Vector3.Right, MathHelper.PiOver2);
Space.Add(shoulder);
//Connect the shoulder to the body.
axisJoint = new RevoluteJoint(body, shoulder, shoulder.Position, Vector3.Forward);
//Motorize the connection.
axisJoint.Motor.IsActive = true;
axisJoint.Motor.Settings.VelocityMotor.GoalVelocity = 1;
Space.Add(axisJoint);
//Connect the arm to the shoulder.
axisJoint = new RevoluteJoint(shoulder, arm, shoulder.Position + new Vector3(0, .6f, 0), Vector3.Forward);
Space.Add(axisJoint);
//Connect the arm to the body.
pointOnLineJoint = new PointOnLineJoint(arm, body, arm.Position, Vector3.Up, arm.Position + new Vector3(0, -.4f, 0));
Space.Add(pointOnLineJoint);
//************* Third Arm *************//
arm = new Box(body.Position + new Vector3(-1.8f, -.5f, -1.5f), .5f, 3, .2f, 20);
Space.Add(arm);
shoulder = new Cylinder(body.Position + new Vector3(-1.8f, .3f, -1.25f), .1f, .7f, 10);
shoulder.OrientationMatrix = Matrix3x3.CreateFromAxisAngle(Vector3.Right, MathHelper.PiOver2);
Space.Add(shoulder);
//Connect the shoulder to the body.
axisJoint = new RevoluteJoint(body, shoulder, shoulder.Position, Vector3.Forward);
//Motorize the connection.
axisJoint.Motor.IsActive = true;
axisJoint.Motor.Settings.VelocityMotor.GoalVelocity = 1;
Space.Add(axisJoint);
//Connect the arm to the shoulder.
axisJoint = new RevoluteJoint(shoulder, arm, shoulder.Position + new Vector3(0, .6f, 0), Vector3.Forward);
Space.Add(axisJoint);
//Connect the arm to the body.
pointOnLineJoint = new PointOnLineJoint(arm, body, arm.Position, Vector3.Up, arm.Position + new Vector3(0, -.4f, 0));
Space.Add(pointOnLineJoint);
shoulder.OrientationMatrix *= Matrix3x3.CreateFromAxisAngle(Vector3.Forward, MathHelper.Pi); //Force the walker's legs out of phase.
//************* Fourth Arm *************//
arm = new Box(body.Position + new Vector3(1.8f, -.5f, -1.5f), .5f, 3, .2f, 20);
Space.Add(arm);
shoulder = new Cylinder(body.Position + new Vector3(1.8f, .3f, -1.25f), .1f, .7f, 10);
shoulder.OrientationMatrix = Matrix3x3.CreateFromAxisAngle(Vector3.Right, MathHelper.PiOver2);
Space.Add(shoulder);
//Connect the shoulder to the body.
axisJoint = new RevoluteJoint(body, shoulder, shoulder.Position, Vector3.Forward);
//Motorize the connection.
axisJoint.Motor.IsActive = true;
axisJoint.Motor.Settings.VelocityMotor.GoalVelocity = 1;
Space.Add(axisJoint);
//Connect the arm to the shoulder.
axisJoint = new RevoluteJoint(shoulder, arm, shoulder.Position + new Vector3(0, .6f, 0), Vector3.Forward);
Space.Add(axisJoint);
//Connect the arm to the body.
pointOnLineJoint = new PointOnLineJoint(arm, body, arm.Position, Vector3.Up, arm.Position + new Vector3(0, -.4f, 0));
Space.Add(pointOnLineJoint);
//Add some ground.
Space.Add(new Box(new Vector3(0, -3.5f, 0), 20f, 1, 20f));
game.Camera.Position = new Vector3(0, 2, 20);
}
public static void Test()
{
var random = new Random(4);
var timer = new Stopwatch();
var symmetricVectorSandwichTime = 0.0;
var symmetricWideVectorSandwichTime = 0.0;
var triangularWideVectorSandwichTime = 0.0;
var symmetricWide2x3SandwichTime = 0.0;
var triangularWide2x3SandwichTime = 0.0;
var symmetricSkewSandwichTime = 0.0;
var symmetricWideSkewSandwichTime = 0.0;
var triangularWideSkewSandwichTime = 0.0;
var symmetricRotationSandwichTime = 0.0;
var symmetricWideRotationSandwichTime = 0.0;
var triangularWideRotationSandwichTime = 0.0;
var symmetricInvertTime = 0.0;
var symmetricWideInvertTime = 0.0;
var triangularWideInvertTime = 0.0;
for (int i = 0; i < 1000; ++i)
{
var axis = Vector3.Normalize(new Vector3((float)random.NextDouble() * 2 - 1, (float)random.NextDouble() * 2 - 1, (float)random.NextDouble() * 2 - 1));
Vector3Wide.Broadcast(axis, out var axisWide);
var rotation = Matrix3x3.CreateFromAxisAngle(axis, (float)random.NextDouble());
Matrix3x3Wide rotationWide;
Vector3Wide.Broadcast(rotation.X, out rotationWide.X);
Vector3Wide.Broadcast(rotation.Y, out rotationWide.Y);
Vector3Wide.Broadcast(rotation.Z, out rotationWide.Z);
var m2x3Wide = new Matrix2x3Wide()
{
X = axisWide, Y = new Vector3Wide {
X = -axisWide.Y, Y = axisWide.Z, Z = axisWide.X
}
};
var triangular = new Symmetric3x3
{
XX = (float)random.NextDouble() * 2 + 1,
YX = (float)random.NextDouble() * 1 + 1,
YY = (float)random.NextDouble() * 2 + 1,
ZX = (float)random.NextDouble() * 1 + 1,
ZY = (float)random.NextDouble() * 1 + 1,
ZZ = (float)random.NextDouble() * 2 + 1,
};
Symmetric3x3Wide triangularWide;
triangularWide.XX = new Vector <float>(triangular.XX);
triangularWide.YX = new Vector <float>(triangular.YX);
triangularWide.YY = new Vector <float>(triangular.YY);
triangularWide.ZX = new Vector <float>(triangular.ZX);
triangularWide.ZY = new Vector <float>(triangular.ZY);
triangularWide.ZZ = new Vector <float>(triangular.ZZ);
var symmetric = new Matrix3x3
{
X = new Vector3(triangular.XX, triangular.YX, triangular.ZX),
Y = new Vector3(triangular.YX, triangular.YY, triangular.ZY),
Z = new Vector3(triangular.ZX, triangular.ZY, triangular.ZZ),
};
Matrix3x3Wide symmetricWide;
Vector3Wide.Broadcast(symmetric.X, out symmetricWide.X);
Vector3Wide.Broadcast(symmetric.Y, out symmetricWide.Y);
Vector3Wide.Broadcast(symmetric.Z, out symmetricWide.Z);
var symmetricVectorSandwich = new SymmetricVectorSandwich()
{
v = axis, symmetric = symmetric
};
var symmetricWideVectorSandwich = new SymmetricWideVectorSandwich()
{
v = axisWide, symmetric = symmetricWide
};
var triangularWideVectorSandwich = new TriangularWideVectorSandwich()
{
v = axisWide, triangular = triangularWide
};
var symmetricWide2x3Sandwich = new SymmetricWide2x3Sandwich()
{
m = m2x3Wide, symmetric = symmetricWide
};
var triangularWide2x3Sandwich = new TriangularWide2x3Sandwich()
{
m = m2x3Wide, triangular = triangularWide
};
var symmetricSkewSandwich = new SymmetricSkewSandwich()
{
v = axis, symmetric = symmetric
};
var symmetricWideSkewSandwich = new SymmetricWideSkewSandwich()
{
v = axisWide, symmetric = symmetricWide
};
var triangularWideSkewSandwich = new TriangularWideSkewSandwich()
{
v = axisWide, triangular = triangularWide
};
var symmetricSandwich = new SymmetricRotationSandwich()
{
rotation = rotation, symmetric = symmetric
};
var symmetricWideSandwich = new SymmetricRotationSandwichWide()
{
rotation = rotationWide, symmetric = symmetricWide
};
var triangularWideSandwich = new TriangularRotationSandwichWide()
{
rotation = rotationWide, triangular = triangularWide
};
var symmetricInvert = new SymmetricInvert()
{
symmetric = symmetric
};
var symmetricWideInvert = new SymmetricInvertWide()
{
symmetric = symmetricWide
};
var triangularWideInvert = new TriangularInvertWide()
{
triangular = triangularWide
};
const int innerIterations = 100000;
symmetricVectorSandwichTime += TimeTest(innerIterations, ref symmetricVectorSandwich);
symmetricWideVectorSandwichTime += TimeTest(innerIterations, ref symmetricWideVectorSandwich);
triangularWideVectorSandwichTime += TimeTest(innerIterations, ref triangularWideVectorSandwich);
symmetricWide2x3SandwichTime += TimeTest(innerIterations, ref symmetricWide2x3Sandwich);
triangularWide2x3SandwichTime += TimeTest(innerIterations, ref triangularWide2x3Sandwich);
symmetricSkewSandwichTime += TimeTest(innerIterations, ref symmetricSkewSandwich);
symmetricWideSkewSandwichTime += TimeTest(innerIterations, ref symmetricWideSkewSandwich);
triangularWideSkewSandwichTime += TimeTest(innerIterations, ref triangularWideSkewSandwich);
symmetricRotationSandwichTime += TimeTest(innerIterations, ref symmetricSandwich);
symmetricWideRotationSandwichTime += TimeTest(innerIterations, ref symmetricWideSandwich);
triangularWideRotationSandwichTime += TimeTest(innerIterations, ref triangularWideSandwich);
symmetricInvertTime += TimeTest(innerIterations, ref symmetricInvert);
symmetricWideInvertTime += TimeTest(innerIterations, ref symmetricWideInvert);
triangularWideInvertTime += TimeTest(innerIterations, ref triangularWideInvert);
Compare(symmetricVectorSandwich.result, ref symmetricWideVectorSandwich.result);
Compare(symmetricVectorSandwich.result, ref triangularWideVectorSandwich.result);
Compare(ref symmetricWide2x3Sandwich.result, ref triangularWide2x3Sandwich.result);
Compare(ref symmetricSkewSandwich.result, ref symmetricWideSkewSandwich.result);
Compare(ref symmetricSkewSandwich.result, ref triangularWideSkewSandwich.result);
Compare(ref symmetricSandwich.result, ref symmetricWideSandwich.result);
Compare(ref symmetricSandwich.result, ref triangularWideSandwich.result);
Compare(ref symmetricInvert.result, ref symmetricWideInvert.result);
Compare(ref symmetricInvert.result, ref triangularWideInvert.result);
}
Console.WriteLine($"Symmetric vector sandwich: {symmetricVectorSandwichTime}");
Console.WriteLine($"Symmetric wide vector sandwich: {symmetricWideVectorSandwichTime}");
Console.WriteLine($"Triangular wide vector sandwich: {triangularWideVectorSandwichTime}");
Console.WriteLine($"Symmetric wide 2x3 sandwich: {symmetricWide2x3SandwichTime}");
Console.WriteLine($"Triangular wide 2x3 sandwich: {triangularWide2x3SandwichTime}");
Console.WriteLine($"Symmetric skew sandwich: {symmetricSkewSandwichTime}");
Console.WriteLine($"Symmetric wide skew sandwich: {symmetricWideSkewSandwichTime}");
Console.WriteLine($"Triangular wide skew sandwich: {triangularWideSkewSandwichTime}");
Console.WriteLine($"Symmetric rotation sandwich: {symmetricRotationSandwichTime}");
Console.WriteLine($"Symmetric wide rotation sandwich: {symmetricWideRotationSandwichTime}");
Console.WriteLine($"Triangular wide rotation sandwich: {triangularWideRotationSandwichTime}");
Console.WriteLine($"Symmetric invert: {symmetricInvertTime}");
Console.WriteLine($"Symmetric wide invert: {symmetricWideInvertTime}");
Console.WriteLine($"Triangular wide invert: {triangularWideInvertTime}");
}
/// <summary>
/// Initializes a new instance of the HingeJoint class.
/// </summary>
/// <param name="world">The world class where the constraints get added to.</param>
/// <param name="body1">The first body connected to the second one.</param>
/// <param name="body2">The second body connected to the first one.</param>
/// <param name="position">The position in world space where both bodies get connected.</param>
/// <param name="hingeAxis">The axis if the hinge.</param>
public LimitedHingeJoint(World world, RigidBody body1, RigidBody body2, Vector3 position, Vector3 hingeAxis,
float hingeFwdAngle, float hingeBckAngle)
: base(world)
{
// Create the hinge first, two point constraints
worldPointConstraint = new PointOnPoint[2];
hingeAxis *= 0.5f;
Vector3 pos1 = position; Vector3.Add(ref pos1, ref hingeAxis, out pos1);
Vector3 pos2 = position; Vector3.Subtract(ref pos2, ref hingeAxis, out pos2);
worldPointConstraint[0] = new PointOnPoint(body1, body2, pos1);
worldPointConstraint[1] = new PointOnPoint(body1, body2, pos2);
// Now the limit, one max distance constraint
hingeAxis.Normalize();
// choose a direction that is perpendicular to the hinge
Vector3 perpDir = Vector3.up;
if (Vector3.Dot(perpDir, hingeAxis) > 0.1f)
{
perpDir = Vector3.right;
}
// now make it perpendicular to the hinge
Vector3 sideAxis = Vector3.Cross(hingeAxis, perpDir);
perpDir = Vector3.Cross(sideAxis, hingeAxis);
perpDir.Normalize();
// the length of the "arm" TODO take this as a parameter? what's
// the effect of changing it?
float len = 10.0f * 3;
// Choose a position using that dir. this will be the anchor point
// for body 0. relative to hinge
Vector3 hingeRelAnchorPos0 = perpDir * len;
// anchor point for body 2 is chosen to be in the middle of the
// angle range. relative to hinge
float angleToMiddle = 0.5f * (hingeFwdAngle - hingeBckAngle);
Vector3 hingeRelAnchorPos1 = Vector3.Transform(hingeRelAnchorPos0, Matrix3x3.CreateFromAxisAngle(hingeAxis, -angleToMiddle / 360.0f * 2.0f * Mathf.PI));
// work out the "string" length
float hingeHalfAngle = 0.5f * (hingeFwdAngle + hingeBckAngle);
float allowedDistance = len * 2.0f * Mathf.Sin(hingeHalfAngle * 0.5f / 360.0f * 2.0f * Mathf.PI);
Vector3 hingePos = body1.Position;
Vector3 relPos0c = hingePos + hingeRelAnchorPos0;
Vector3 relPos1c = hingePos + hingeRelAnchorPos1;
distance = new PointPointDistance(body1, body2, relPos0c, relPos1c);
distance.Distance = allowedDistance;
distance.Behavior = PointPointDistance.DistanceBehavior.LimitMaximumDistance;
}
/// <summary>
/// Constructs a new demo.
/// </summary>
/// <param name="game">Game owning this demo.</param>
public ScaleDemo(DemosGame game)
: base(game)
{
//Pick a scale!
//Beware: If you go too far (particularly 0.01 and lower) issues could start to crop up.
float scale = 1;
//Load in mesh data and create the collision mesh.
//The 'mesh' will be a supergiant triangle.
//Triangles in meshes have a collision detection system which bypasses most numerical issues for collisions on the face of the triangle.
//Edge collisions fall back to the general case collision detection system which is susceptible to numerical issues at extreme scales.
//For our simulation, the edges will be too far away to worry about!
Vector3[] vertices;
int[] indices;
vertices = new Vector3[] { new Vector3(-10000, 0, -10000), new Vector3(-10000, 0, 20000), new Vector3(20000, 0, -10000) };
indices = new int[] { 2, 1, 0 };
var staticMesh = new StaticMesh(vertices, indices, new AffineTransform(Matrix3x3.CreateFromAxisAngle(Vector3.Up, MathHelper.Pi), new Vector3(0, 0, 0)));
staticMesh.Sidedness = TriangleSidedness.Counterclockwise;
Space.Add(staticMesh);
game.ModelDrawer.Add(staticMesh);
//Since everything's pretty large, increase the gravity a whole bunch to make things fall fast.
Space.ForceUpdater.Gravity *= scale;
//Change the various engine tuning factors so that collision detection and collision response handle the changed scale better.
ConfigurationHelper.ApplyScale(Space, scale);
//When dealing with objects that generally have high velocities and accelerations relative to their size, having a shorter time step duration can boost quality
//a whole lot. Once the configuration is set properly, most of any remaining 'unsmoothness' in the simulation is due to a lack of temporal resolution;
//one discrete step can take an object from a valid state to an unpleasing state due to the high rates of motion.
//To simulate the same amount of time with a smaller time step duration requires taking more time steps.
//This is a quality-performance tradeoff. If you want to do this, set the time step duration like so:
//Space.TimeStepSettings.TimeStepDuration = 1 / 120f;
//And then, in the update, either call the Space.Update() method proportionally more often or use the Space.Update(dt) version, which takes as many timesteps are necessary to simulate dt time.
//Watch out: when using the internal timestepping method, you may notice slight motion jitter since the number of updates per frame isn't fixed. Interpolation buffers can be used
//to address this; check the Asynchronous Update documentation for more information on using internal time stepping.
//[Asynchronously updating isn't required to use internal timestepping, but it is a common use case.]
//Dump some boxes on top of it for fun.
int numColumns = 8;
int numRows = 8;
int numHigh = 1;
float separation = 2 * scale;
float baseWidth = 0.5f;
float baseHeight = 1;
float baseLength = 1.5f;
Entity toAdd;
for (int i = 0; i < numRows; i++)
{
for (int j = 0; j < numColumns; j++)
{
for (int k = 0; k < numHigh; k++)
{
toAdd = new Box(
new Vector3(
separation * i - numRows * separation / 2,
2 * scale + k * separation,
separation * j - numColumns * separation / 2),
baseWidth * scale, baseHeight * scale, baseLength * scale, 15);
Space.Add(toAdd);
}
}
}
//Dump some stuff on top of it that use general case collision detection when they collide with boxes.
numColumns = 3;
numRows = 3;
numHigh = 4;
separation = 2 * scale;
baseWidth = 1f;
baseHeight = 1;
for (int i = 0; i < numRows; i++)
{
for (int j = 0; j < numColumns; j++)
{
for (int k = 0; k < numHigh; k++)
{
toAdd = new Cylinder(
new Vector3(
separation * i - numRows * separation / 2,
8 * scale + k * separation,
separation * j - numColumns * separation / 2),
baseHeight * scale, 0.5f * baseWidth * scale, 15);
Space.Add(toAdd);
}
}
}
game.Camera.Position = scale * new Vector3(0, 4, 10);
originalCameraSpeed = freeCameraControlScheme.Speed;
freeCameraControlScheme.Speed *= scale;
}
