public PistonConstraint(
int indexA,
int indexB,
SimulationObject[] simulationObject,
Vector3 startAnchorPosition,
Vector3 pistonAxis,
double restoreCoefficient,
double springCoefficient)
{
IndexA = indexA;
IndexB = indexB;
KeyIndex = GetHashCode();
RestoreCoefficient = restoreCoefficient;
SpringCoefficient = springCoefficient;
StartAnchorPoint = startAnchorPosition;
PistonAxis = -1.0 * pistonAxis.Normalize ();
SimulationObject objectA = simulationObject[IndexA];
SimulationObject objectB = simulationObject[IndexB];
Vector3 relativePos = objectA.RotationMatrix *
(startAnchorPosition - objectA.StartPosition);
AnchorPoint = relativePos + objectA.Position;
StartErrorAxis1 = objectA.RotationMatrix.Transpose() *
(AnchorPoint - objectA.Position);
StartErrorAxis2 = objectB.RotationMatrix.Transpose() *
(AnchorPoint - objectB.Position);
RelativeOrientation = objectB.RotationStatus.Inverse() *
objectA.RotationStatus;
}
public BallAndSocketConstraint(
int indexA,
int indexB,
SimulationObject[] simulationObject,
Vector3 startAnchorPosition,
double restoreCoefficient,
double springCoefficient)
{
IndexA = indexA;
IndexB = indexB;
KeyIndex = GetHashCode();
RestoreCoefficient = restoreCoefficient;
SpringCoefficient = springCoefficient;
StartAnchorPoint = startAnchorPosition;
SimulationObject objectA = simulationObject[IndexA];
SimulationObject objectB = simulationObject[IndexB];
Vector3 relativePos = startAnchorPosition - objectA.StartPosition;
relativePos = objectA.RotationMatrix * relativePos;
AnchorPoint = relativePos + objectA.Position;
StartErrorAxis1 = objectA.RotationMatrix.Transpose() *
(AnchorPoint - objectA.Position);
StartErrorAxis2 = objectB.RotationMatrix.Transpose() *
(AnchorPoint - objectB.Position);
}
public FixedJointConstraint(
int indexA,
int indexB,
SimulationObject[] simulationObject,
double restoreCoefficient,
double springCoefficient)
{
IndexA = indexA;
IndexB = indexB;
KeyIndex = GetHashCode();
SpringCoefficient = springCoefficient;
RestoreCoefficient = restoreCoefficient;
SimulationObject objectA = simulationObject[IndexA];
SimulationObject objectB = simulationObject[IndexB];
StartAnchorPoint = (objectB.Position - objectA.Position) * 0.5;
Vector3 relativePos = StartAnchorPoint - objectA.StartPosition;
relativePos = objectA.RotationMatrix * relativePos;
AnchorPoint = relativePos + objectA.Position;
StartErrorAxis1 = objectA.RotationMatrix.Transpose() *
(AnchorPoint - objectA.Position);
StartErrorAxis2 = objectB.RotationMatrix.Transpose() *
(AnchorPoint - objectB.Position);
RelativeOrientation = objectB.RotationStatus.Inverse() *
objectA.RotationStatus;
}
public List<CollisionPoint> GetManifoldPoints(
SimulationObject objectA,
SimulationObject objectB,
CollisionPoint collisionPoint)
{
List<Vector3> collisionA = getNearestPoint (
objectA,
collisionPoint.CollisionPointA,
collisionPoint.CollisionNormal);
List<Vector3> collisionB = getNearestPoint (
objectB,
collisionPoint.CollisionPointB,
collisionPoint.CollisionNormal);
List<CollisionPoint> collisionPointsList = findCollisionPoints (
collisionA.ToArray (),
collisionB.ToArray (),
collisionPoint.CollisionNormal,
collisionPoint);
collisionA.Clear ();
collisionB.Clear ();
return collisionPointsList;
}
public static AABB UpdateAABB(
SimulationObject simObject)
{
Vector3 vertexPos = GetVertexPosition(simObject, 0);
double xMax = vertexPos.x;
double xMin = vertexPos.x;
double yMax = vertexPos.y;
double yMin = vertexPos.y;
double zMax = vertexPos.z;
double zMin = vertexPos.z;
for (int i = 1; i < simObject.RelativePositions.Length; i++)
{
Vector3 vertex = GetVertexPosition(simObject, i);
if (vertex.x < xMin)
xMin = vertex.x;
else if (vertex.x > xMax)
xMax = vertex.x;
if (vertex.y < yMin)
yMin = vertex.y;
else if (vertex.y > yMax)
yMax = vertex.y;
if (vertex.z < zMin)
zMin = vertex.z;
else if (vertex.z > zMax)
zMax = vertex.z;
}
return new AABB(xMin, xMax, yMin, yMax, zMin, zMax, false);
}
public static Vector3 GetFixedAngularError(
SimulationObject objectA,
SimulationObject objectB,
Quaternion relativeOrientation)
{
Quaternion currentRelativeOrientation = objectB.RotationStatus.Inverse () *
objectA.RotationStatus;
Quaternion relativeOrientationError = relativeOrientation.Inverse () *
currentRelativeOrientation;
var angularError = new Vector3 (
relativeOrientationError.b,
relativeOrientationError.c,
relativeOrientationError.d);
if (relativeOrientationError.a < 0.0)
{
angularError = new Vector3 (
-angularError.x,
-angularError.y,
-angularError.z);
}
return objectA.RotationMatrix * angularError;
}
public static Vector3 GetVertexPosition(
SimulationObject obj,
int index)
{
return
obj.Position +
(obj.RotationMatrix * obj.RelativePositions[index]);
}
private SimulationObject[] getSimulationObjects()
{
SimulationObject[] objects = new SimulationObject[1];
#region Terrain Base
objects[0] = new SimulationObject(
ObjectType.StaticRigidBody,
geometry: GetObjectGeometry("cube.obj", 25),
mass: 1000.0,
position: new Vector3(0.0, -4.0, 0.0),
rotationStatus: new Quaternion(new Vector3(0.0, 0.0, 0.0), 0.0));
objects[0].SetLinearVelocity(new Vector3(0.0, 0.0, 0.0));
objects[0].SetAngularVelocity(new Vector3(0.0, 0.0, 0.0));
objects[0].SetRestitutionCoeff(0.1);
objects[0].SetDynamicFrictionCoeff(1.0);
objects[0].SetStaticFrictionCoeff(1.0);
objects[0].SetExcludeFromCollisionDetection(false);
Vector3[] vertexPosition = Array.ConvertAll(objects[0].ObjectGeometry[0].VertexPosition,
item => item.Vertex);
var inertiaTensor = new InertiaTensor(
vertexPosition,
objects[0].ObjectGeometry[0].Triangle,
objects[0].Mass);
//Traslo per normalizzare l'oggetto rispetto al suo centro di massa
for (int j = 0; j < objects[0].ObjectGeometry[0].VertexPosition.Length; j++)
{
objects[0].ObjectGeometry[0].SetVertexPosition(
vertexPosition[j] - inertiaTensor.GetMassCenter(),
j);
}
var inertiaTensor1 = new InertiaTensor(
vertexPosition,
objects[0].ObjectGeometry[0].Triangle,
objects[0].Mass);
objects[0].SetStartPosition(inertiaTensor1.GetMassCenter());
objects[0].SetBaseInertiaTensor(inertiaTensor1.GetInertiaTensor());
objects[0].SetRotationMatrix(Quaternion.ConvertToMatrix(Quaternion.Normalize(objects[0].RotationStatus)));
objects[0].SetInertiaTensor((objects[0].RotationMatrix * objects[0].BaseInertiaTensor) *
Matrix3x3.Transpose(objects[0].RotationMatrix));
for (int j = 0; j < objects[0].ObjectGeometry[0].VertexPosition.Length; j++)
{
Vector3 relPositionRotate = objects[0].RotationMatrix * objects[0].RelativePositions[j];
objects[0].ObjectGeometry[0].SetVertexPosition(objects[0].Position + relPositionRotate, j);
}
#endregion
return objects;
}
/// <summary>
/// Gets the farthest vertex point between two input objects.
/// </summary>
/// <returns>The farthest point.</returns>
/// <param name="objA">Object a.</param>
/// <param name="objB">Object b.</param>
private Support GetFarthestPoint(
SimulationObject objA,
SimulationObject objB)
{
int indexA = 0;
int indexB = objB.RelativePositions.Length / 2;
return new Support(
Helper.GetVertexPosition(objA,indexA) - Helper.GetVertexPosition(objB, indexB),
indexA,
indexB);
}
public Hinge2Constraint(
int indexA,
int indexB,
SimulationObject[] simulationObject,
Vector3 startAnchorPosition,
Vector3 hingeAxis,
Vector3 rotationAxis,
double restoreCoefficient,
double springCoefficientHingeAxis,
double springCoefficient)
{
IndexA = indexA;
IndexB = indexB;
KeyIndex = GetHashCode();
RestoreCoefficient = restoreCoefficient;
SpringCoefficientHingeAxis = springCoefficientHingeAxis;
SpringCoefficient = springCoefficient;
StartAnchorPoint = startAnchorPosition;
HingeAxis = hingeAxis.Normalize ();
RotationAxis = rotationAxis.Normalize ();
SimulationObject objectA = simulationObject[IndexA];
SimulationObject objectB = simulationObject[IndexB];
Vector3 relativePos = startAnchorPosition - objectA.StartPosition;
relativePos = objectA.RotationMatrix * relativePos;
AnchorPoint = relativePos + objectA.Position;
StartErrorAxis1 = objectA.RotationMatrix.Transpose() *
(AnchorPoint - objectA.Position);
StartErrorAxis2 = objectB.RotationMatrix.Transpose() *
(AnchorPoint - objectB.Position);
Vector3 rHingeAxis = objectA.RotationMatrix * HingeAxis;
Vector3 rRotationAxis = objectB.RotationMatrix * RotationAxis;
RelativeOrientation1 = CalculateRelativeOrientation(
rHingeAxis,
rRotationAxis,
objectA.RotationStatus);
RelativeOrientation2 = CalculateRelativeOrientation(
rRotationAxis,
rHingeAxis,
objectB.RotationStatus);
}
/// <summary>
/// Runs the test collision.
/// </summary>
/// <returns>The test collision.</returns>
/// <param name="objects">Objects.</param>
/// <param name="minDistance">Minimum distance.</param>
public List<CollisionPointStructure> Execute(
SimulationObject[] objects,
double minDistance)
{
if (collisionEngineParameters.ActivateSweepAndPrune)
{
return SweepAndPruneBroadPhase(
objects,
minDistance);
}
else
{
return BruteForceBroadPhase(
objects,
minDistance);
}
}
private void GetEpaVertexFromMinkowsky(
SupportTriangle triangle,
SimulationObject shape1,
SimulationObject shape2,
ref EngineCollisionPoint epaCollisionPoint)
{
Vector3 a1 = Helper.GetVertexPosition(shape1, triangle.a.a);
Vector3 ba1 = Helper.GetVertexPosition(shape1, triangle.b.a) - a1;
Vector3 ca1 = Helper.GetVertexPosition(shape1, triangle.c.a) - a1;
Vector3 a2 = Helper.GetVertexPosition(shape2, triangle.a.b);
Vector3 ba2 = Helper.GetVertexPosition(shape2, triangle.b.b) - a2;
Vector3 ca2 = Helper.GetVertexPosition(shape2, triangle.c.b) - a2;
epaCollisionPoint.SetA (a1 + (ba1 * triangle.s) + (ca1 * triangle.t));
epaCollisionPoint.SetB (a2 + (ba2 * triangle.s) + (ca2 * triangle.t));
}
/// <summary>
/// Gets the nearest point from collision point.
/// </summary>
/// <returns>The nearest point.</returns>
/// <param name="shape">Shape.</param>
/// <param name="collisionPoint">Collision point.</param>
private List<Vector3> getNearestPoint(
SimulationObject shape,
Vector3 collisionPoint,
Vector3 planeNormal)
{
var collisionPoints = new List<Vector3> ();
Vector3 normal = Vector3.Normalize(planeNormal);
for (int i = 0; i < shape.ObjectGeometry[0].VertexPosition.Length; i++)
{
Vector3 nt = Vector3.Normalize (Helper.GetVertexPosition(shape, i) - collisionPoint);
if (Math.Abs (Vector3.Dot (nt, normal)) < ManifoldPlaneTolerance)
collisionPoints.Add (Helper.GetVertexPosition(shape, i));
}
return collisionPoints;
}
public static double GetAngle(
SimulationObject simulationObjectA,
SimulationObject simulationObjectB,
Quaternion relativeRotation,
Vector3 rotationAxis)
{
Quaternion currentRelativeOrientation = simulationObjectA.RotationStatus.Inverse () *
simulationObjectB.RotationStatus;
Quaternion relativeOrientation = relativeRotation.Inverse () *
currentRelativeOrientation;
var quaternionVectorPart = new Vector3 (
relativeOrientation.b,
relativeOrientation.c,
relativeOrientation.d);
quaternionVectorPart = simulationObjectA.RotationMatrix * quaternionVectorPart;
return GetRotationAngle (
quaternionVectorPart,
relativeOrientation.a,
rotationAxis);
}
/// <summary>
/// Ritorna la distanza se distanza 0.0f allora vi è intersezione o compenetrazione tra gli oggetti,
/// se distanza 0.0 allora i due oggetti non collidono
/// </summary>
/// <returns>The GJK algorithm.</returns>
/// <param name="shape1">Shape1.</param>
/// <param name="shape2">Shape2.</param>
/// <param name="cp">Cp.</param>
/// <param name="isIntersection">If set to <c>true</c> is itersection.</param>
private double ExecuteGJKAlgorithm(
SimulationObject shape1,
SimulationObject shape2,
int geometryIndexA,
int geometryIndexB,
ref Vector3 collisionNormal,
ref CollisionPoint cp,
ref List<SupportTriangle> triangles,
ref Vector3 centroid,
ref bool isIntersection)
{
double minDistance = double.MaxValue;
int minTriangleIndex = -1;
var result = new EngineCollisionPoint();
var oldDirection = new Vector3();
var simplex = new Simplex();
//Primo punto del simplex
simplex.Support.Add(GetFarthestPoint(shape1, shape2));
//Secondo punto del simplex
Vector3 direction = Vector3.Normalize(simplex.Support[0].s * -1.0);
if (!simplex.AddSupport(Helper.GetMinkowskiFarthestPoint(shape1, shape2, geometryIndexA, geometryIndexB, direction)))
return -1.0;
//Terzo punto del simplex
direction = Vector3.Normalize(GetDirectionOnSimplex2(simplex));
if(!simplex.AddSupport(Helper.GetMinkowskiFarthestPoint(shape1, shape2, geometryIndexA, geometryIndexB, direction)))
return -1.0;
//Quarto punto del simplex
direction = Vector3.Normalize(GeometryUtilities.CalculateNormal(
simplex.Support[0].s,
simplex.Support[1].s,
simplex.Support[2].s));
if (!simplex.AddSupport(Helper.GetMinkowskiFarthestPoint(shape1, shape2, geometryIndexA, geometryIndexB, direction)))
simplex.AddSupport(Helper.GetMinkowskiFarthestPoint(shape1, shape2, geometryIndexA, geometryIndexB, -1.0 * direction));
//Costruisco il poliedro
centroid = Helper.SetStartTriangle(
ref triangles,
simplex.Support.ToArray());
//Verifico che l'origine sia contenuta nel poliedro
if (Helper.IsInConvexPoly(origin, triangles))
{
isIntersection = true;
return -1.0;
}
Vector3 triangleDistance = GetMinDistance(ref triangles, origin, ref minTriangleIndex);
result.SetDist(triangleDistance);
result.SetNormal(Vector3.Normalize(triangleDistance));
Helper.GetVertexFromMinkowsky(triangles[minTriangleIndex], shape1, shape2, ref result);
minDistance = triangleDistance.Length();
for (int i = 0; i < MaxIterations; i++)
{
direction = -1.0 * triangleDistance.Normalize();
if (Vector3.Length(direction) < constTolerance)
{
direction = origin - centroid;
}
if (direction == oldDirection)
break;
oldDirection = direction;
if (!simplex.AddSupport(Helper.GetMinkowskiFarthestPoint(shape1, shape2, geometryIndexA, geometryIndexB, direction)))
{
for (int j = 0; j < triangles.Count; j++)
{
direction = triangles[j].normal;
if (!simplex.AddSupport(Helper.GetMinkowskiFarthestPoint(shape1, shape2, geometryIndexA, geometryIndexB, direction)))
{
if (simplex.AddSupport(Helper.GetMinkowskiFarthestPoint(shape1, shape2, geometryIndexA, geometryIndexB, -1.0 * direction)))
break;
continue;
}
break;
}
}
triangles = Helper.AddPointToConvexPolygon(triangles, simplex.Support[simplex.Support.Count - 1], centroid);
//Verifico che l'origine sia contenuta nel poliedro
if (Helper.IsInConvexPoly(origin, triangles))
{
isIntersection = true;
return -1.0;
}
triangleDistance = GetMinDistance(ref triangles, origin, ref minTriangleIndex);
double mod = triangleDistance.Length();
if (mod < minDistance)
{
result.SetDist(triangleDistance);
result.SetNormal(Vector3.Normalize(triangleDistance));
Helper.GetVertexFromMinkowsky(triangles[minTriangleIndex], shape1, shape2, ref result);
minDistance = mod;
}
}
collisionNormal = -1.0 * result.normal;
cp = new CollisionPoint(
result.a,
result.b,
collisionNormal);
return minDistance;
}
List<JacobianContact> getLinearLimit(
SimulationObject simulationObjectA,
SimulationObject simulationObjectB,
Vector3 sliderAxis,
Vector3 r1,
Vector3 r2)
{
var linearConstraints = new List<JacobianContact>();
if (LinearLimitMin.HasValue &&
LinearLimitMax.HasValue)
{
linearConstraints.Add(
JacobianCommon.GetLinearLimit(
IndexA,
IndexB,
simulationObjectA,
simulationObjectB,
sliderAxis,
r1,
r2,
RestoreCoefficient,
0.0,
LinearLimitMin.Value,
LinearLimitMax.Value));
}
return linearConstraints;
}
List<JacobianContact> getAnguarLimit(
SimulationObject simulationObjectA,
SimulationObject simulationObjectB,
Vector3 sliderAxis)
{
var angularConstraints = new List<JacobianContact>();
if (AngularLimitMin.HasValue &&
AngularLimitMax.HasValue)
{
double angle = JacobianCommon.GetAngle(
simulationObjectA,
simulationObjectB,
RelativeOrientation,
PistonAxis);
JacobianContact? jContact =
JacobianCommon.GetAngularLimit (
IndexA,
IndexB,
angle,
RestoreCoefficient,
0.0,
simulationObjectA,
simulationObjectB,
sliderAxis,
AngularLimitMin.Value,
AngularLimitMax.Value);
if (jContact != null)
angularConstraints.Add (jContact.Value);
}
return angularConstraints;
}
private CollisionPointStructure NarrowPhaseCollisionControl(
GJKOutput gjkOutput,
SimulationObject A,
SimulationObject B,
int indexA,
int indexB,
int geometryIndexA,
int geometryIndexB,
double minDistance)
{
if (!gjkOutput.Intersection &&
gjkOutput.CollisionDistance <= minDistance)
{
var mpg = new ManifoldPointsGenerator(
collisionEngineParameters.ManifoldPointNumber,
collisionEngineParameters.GJKManifoldTolerance,
collisionEngineParameters.ManifoldProjectionTolerance);
if (gjkOutput.CollisionNormal.Length() < 1E-15)
return null;
List<CollisionPoint> collisionPointsList = mpg.GetManifoldPoints(
A,
B,
gjkOutput.CollisionPoint);
return new CollisionPointStructure(
indexA,
indexB,
gjkOutput.Intersection,
gjkOutput.CollisionDistance,
gjkOutput.CollisionPoint,
collisionPointsList.ToArray());
}
if (gjkOutput.Intersection)
{
EPAOutput epaOutput = compenetrationCollisionEngine.Execute(
A,
B,
geometryIndexA,
geometryIndexB,
gjkOutput.SupportTriangles,
gjkOutput.Centroid);
if (epaOutput.CollisionPoint.CollisionNormal.Length() < 1E-15)
return null;
var mpg = new ManifoldPointsGenerator(
collisionEngineParameters.ManifoldPointNumber,
collisionEngineParameters.EPAManifoldTolerance,
collisionEngineParameters.ManifoldProjectionTolerance);
List<CollisionPoint> collisionPointsList = mpg.GetManifoldPoints(
A,
B,
epaOutput.CollisionPoint);
return new CollisionPointStructure(
indexA,
indexB,
gjkOutput.Intersection,
epaOutput.CompenetrationDistance,
epaOutput.CollisionPoint,
collisionPointsList.ToArray());
}
return null;
}
public void AddTorque(SimulationObject[] objects, double torqueAxis1, double torqueAxis2)
{
Vector3 pistonAxis = objects[IndexA].RotationMatrix * PistonAxis;
Vector3 torque = PistonAxis * torqueAxis1;
objects[IndexA].SetTorque(objects[IndexA].TorqueValue + torque);
objects[IndexB].SetTorque(objects[IndexB].TorqueValue - torque);
}
private List<CollisionPointStructure> SweepAndPruneBroadPhase(
SimulationObject[] objects,
double minDistance)
{
var result = new List<CollisionPointStructure> ();
AABB[][] boxs = Array.ConvertAll(objects, item => (item.ObjectGeometry == null) ? null : Array.ConvertAll(item.ObjectGeometry, x => x.AABBox));
List<CollisionPair> collisionPair = sweepAndPruneEngine.Execute (boxs, minDistance);
var lockMe = new object();
Parallel.ForEach(
collisionPair,
new ParallelOptions { MaxDegreeOfParallelism = collisionEngineParameters.MaxThreadNumber },
pair =>
{
CollisionPointStructure collisionPointStruct = NarrowPhase(
objects[pair.objectIndexA],
objects[pair.objectIndexB],
pair.objectIndexA,
pair.objectIndexB,
minDistance);
if (collisionPointStruct != null)
{
lock (lockMe)
{
result.Add(collisionPointStruct);
}
}
});
return result;
}
public void AddTorque(SimulationObject[] objects, double torqueAxis1, double torqueAxis2)
{
Vector3 hingeAxis = objects[IndexA].RotationMatrix * HingeAxis;
Vector3 rotationAxis = objects[IndexB].RotationMatrix * RotationAxis;
Vector3 torque = hingeAxis * torqueAxis1 + rotationAxis * torqueAxis2;
objects[IndexA].SetTorque(objects[IndexA].TorqueValue + torque);
objects[IndexB].SetTorque(objects[IndexB].TorqueValue - torque);
}
private CollisionPointStructure NarrowPhase(
SimulationObject A,
SimulationObject B,
int indexA,
int indexB,
double minDistance)
{
List<CollisionPointStructure> collisionPointStructure = new List<CollisionPointStructure>();
for (int i = 0; i < A.ObjectGeometry.Length; i++)
{
for (int j = 0; j < B.ObjectGeometry.Length; j++)
{
GJKOutput gjkOutput = (collisionEngine.Execute(A, B, i, j));
collisionPointStructure.Add(NarrowPhaseCollisionControl(
gjkOutput,
A,
B,
indexA,
indexB,
i,
j,
minDistance));
}
}
if(collisionPointStructure.Count > 1)
{
}
return collisionPointStructure[0];
}
private List<CollisionPointStructure> BruteForceBroadPhase(
SimulationObject[] objects,
double minDistance)
{
var result = new List<CollisionPointStructure> ();
var lockMe = new object();
Parallel.For (0,
objects.Length,
new ParallelOptions { MaxDegreeOfParallelism = collisionEngineParameters.MaxThreadNumber },
i => {
if (objects [i] != null) {
for (int j = i + 1; j < objects.Length; j++) {
if (objects [j] != null) {
CollisionPointStructure collisionPointStruct = NarrowPhase (
objects [i],
objects [j],
i,
j,
minDistance);
lock (lockMe) {
if (collisionPointStruct != null)
result.Add (collisionPointStruct);
}
}
}
}
});
return result;
}
/// <summary>
/// Builds the slider joint.
/// </summary>
/// <returns>The slider joint.</returns>
/// <param name="simulationObjs">Simulation objects.</param>
public List<JacobianContact> BuildJacobian(
SimulationObject[] simulationObjs,
double? baumStabilization = null)
{
var sliderConstraints = new List<JacobianContact> ();
SimulationObject simulationObjectA = simulationObjs [IndexA];
SimulationObject simulationObjectB = simulationObjs [IndexB];
AnchorPoint = (simulationObjectA.RotationMatrix *
(StartAnchorPoint - simulationObjectA.StartPosition)) +
simulationObjectA.Position;
#region Init Linear
Vector3 sliderAxis = simulationObjectA.RotationMatrix * SliderAxis;
Vector3 t1 = GeometryUtilities.GetPerpendicularVector (sliderAxis).Normalize ();
Vector3 t2 = Vector3.Cross (sliderAxis, t1).Normalize ();
Vector3 r1 = simulationObjectA.RotationMatrix *
StartErrorAxis1;
Vector3 r2 = simulationObjectB.RotationMatrix *
StartErrorAxis2;
Vector3 p1 = simulationObjectA.Position + r1;
Vector3 p2 = simulationObjectB.Position + r2;
Vector3 linearError = p2 - p1;
#endregion
#region Init Angular
Vector3 angularError = JacobianCommon.GetFixedAngularError (
simulationObjectA,
simulationObjectB,
RelativeOrientation);
#endregion
#region Jacobian Constraint
#region Base Constraints
ConstraintType constraintType = ConstraintType.Joint;
if (SpringCoefficient > 0)
constraintType = ConstraintType.SoftJoint;
double constraintLimit = RestoreCoefficient * 2.0 * angularError.x;
//DOF 1
sliderConstraints.Add (JacobianCommon.GetDOF (
IndexA,
IndexB,
new Vector3 (0.0, 0.0, 0.0),
new Vector3 (0.0, 0.0, 0.0),
new Vector3 (-1.0, 0.0, 0.0),
new Vector3 (1.0, 0.0, 0.0),
simulationObjectA,
simulationObjectB,
0.0,
constraintLimit,
SpringCoefficient,
0.0,
constraintType));
//DOF 2
constraintLimit = RestoreCoefficient * 2.0 * angularError.y;
sliderConstraints.Add (JacobianCommon.GetDOF (
IndexA,
IndexB,
new Vector3 (0.0, 0.0, 0.0),
new Vector3 (0.0, 0.0, 0.0),
new Vector3 (0.0, -1.0, 0.0),
new Vector3 (0.0, 1.0, 0.0),
simulationObjectA,
simulationObjectB,
0.0,
constraintLimit,
SpringCoefficient,
0.0,
constraintType));
//DOF 3
constraintLimit = RestoreCoefficient * 2.0 * angularError.z;
sliderConstraints.Add (JacobianCommon.GetDOF (
IndexA,
IndexB,
new Vector3 (0.0, 0.0, 0.0),
new Vector3 (0.0, 0.0, 0.0),
new Vector3 (0.0, 0.0, -1.0),
new Vector3 (0.0, 0.0, 1.0),
simulationObjectA,
simulationObjectB,
0.0,
constraintLimit,
SpringCoefficient,
0.0,
constraintType));
//DOF 4
constraintLimit = RestoreCoefficient * Vector3.Dot (t1,linearError);
sliderConstraints.Add (JacobianCommon.GetDOF (
IndexA,
IndexB,
t1,
-1.0 * t1,
Vector3.Cross (r1, t1),
-1.0 * Vector3.Cross (r2, t1),
simulationObjectA,
simulationObjectB,
0.0,
constraintLimit,
SpringCoefficient,
0.0,
constraintType));
//DOF 5
constraintLimit = RestoreCoefficient * Vector3.Dot (t2,linearError);
sliderConstraints.Add (JacobianCommon.GetDOF (
IndexA,
IndexB,
t2,
-1.0 * t2,
Vector3.Cross (r1, t2),
-1.0 * Vector3.Cross (r2, t2),
simulationObjectA,
simulationObjectB,
0.0,
constraintLimit,
SpringCoefficient,
0.0,
constraintType));
#endregion
#region Limit Constraints
// Limit extraction
if (LinearLimitMin.HasValue &&
LinearLimitMax.HasValue)
{
sliderConstraints.Add (
JacobianCommon.GetLinearLimit(
IndexA,
IndexB,
simulationObjectA,
simulationObjectB,
sliderAxis,
r1,
r2,
RestoreCoefficient,
0.0,
LinearLimitMin.Value,
LinearLimitMax.Value));
}
#endregion
#region Motor Constraint
if (ForceLimit.HasValue &&
SpeedValue.HasValue)
{
sliderConstraints.Add (JacobianCommon.GetDOF (
IndexA,
IndexB,
sliderAxis,
-1.0 * sliderAxis,
new Vector3(),
new Vector3(),
simulationObjectA,
simulationObjectB,
SpeedValue.Value,
0.0,
0.0,
ForceLimit.Value,
ConstraintType.JointMotor));
}
#endregion
#endregion
return sliderConstraints;
}
void IConstraint.AddTorque(SimulationObject[] objects, double torqueAxis1, double torqueAxis2)
{
throw new NotSupportedException();
}
List<JacobianContact> GetAngularLimit(
SimulationObject simulationObjectA,
SimulationObject simulationObjectB,
Vector3 hingeAxis,
Vector3 rotationAxis)
{
var angularConstraint = new List<JacobianContact>();
if (AngularLimitMin1.HasValue &&
AngularLimitMax1.HasValue)
{
double angle1 = GetAngle1(
hingeAxis,
rotationAxis,
HingeAxis,
simulationObjectA.RotationStatus,
RelativeOrientation1);
JacobianContact? jContact =
JacobianCommon.GetAngularLimit (
IndexA,
IndexB,
angle1,
RestoreCoefficient,
0.0,
simulationObjectA,
simulationObjectB,
hingeAxis,
AngularLimitMin1.Value,
AngularLimitMax1.Value);
if (jContact != null)
angularConstraint.Add (jContact.Value);
}
if (AngularLimitMin2.HasValue &&
AngularLimitMax2.HasValue)
{
double angle2 = GetAngle2(
hingeAxis,
rotationAxis,
RotationAxis,
simulationObjectB.RotationStatus,
RelativeOrientation2);
JacobianContact? jContact =
JacobianCommon.GetAngularLimit (
IndexA,
IndexB,
angle2,
RestoreCoefficient,
0.0,
simulationObjectA,
simulationObjectB,
rotationAxis,
AngularLimitMin2.Value,
AngularLimitMax2.Value);
if (jContact != null)
angularConstraint.Add (jContact.Value);
}
return angularConstraint;
}
List<JacobianContact> GetMotorConstraint(
SimulationObject simulationObjectA,
SimulationObject simulationObjectB,
Vector3 hingeAxis,
Vector3 rotationAxis)
{
var motorConstraint = new List<JacobianContact>();
if (SpeedHingeAxisLimit.HasValue &&
ForceHingeAxisLimit.HasValue)
{
motorConstraint.Add(
JacobianCommon.GetDOF(
IndexA,
IndexB,
new Vector3(),
new Vector3(),
-1.0 * hingeAxis,
1.0 * hingeAxis,
simulationObjectA,
simulationObjectB,
SpeedHingeAxisLimit.Value,
0.0,
0.0,
ForceHingeAxisLimit.Value,
ConstraintType.JointMotor));
}
if (SpeedRotationAxisLimit.HasValue &&
ForceRotationAxisLimit.HasValue)
{
motorConstraint.Add(
JacobianCommon.GetDOF(
IndexA,
IndexB,
new Vector3(),
new Vector3(),
-1.0 * rotationAxis,
1.0 * rotationAxis,
simulationObjectA,
simulationObjectB,
SpeedRotationAxisLimit.Value,
0.0,
0.0,
ForceRotationAxisLimit.Value,
ConstraintType.JointMotor));
}
return motorConstraint;
}
List<JacobianContact> getMotorConstraint(
SimulationObject simulationObjectA,
SimulationObject simulationObjectB,
Vector3 sliderAxis)
{
var motorConstraints = new List<JacobianContact>();
if (LinearForceLimit.HasValue &&
LinearSpeedValue.HasValue)
{
motorConstraints.Add(JacobianCommon.GetDOF(
IndexA,
IndexB,
sliderAxis,
-1.0 * sliderAxis,
new Vector3(),
new Vector3(),
simulationObjectA,
simulationObjectB,
LinearSpeedValue.Value,
0.0,
0.0,
LinearForceLimit.Value,
ConstraintType.JointMotor));
}
if (AngularForceLimit.HasValue &&
AngularSpeedValue.HasValue)
{
motorConstraints.Add(JacobianCommon.GetDOF(
IndexA,
IndexB,
new Vector3(),
new Vector3(),
sliderAxis,
-1.0 * sliderAxis,
simulationObjectA,
simulationObjectB,
AngularSpeedValue.Value,
0.0,
0.0,
AngularForceLimit.Value,
ConstraintType.JointMotor));
}
return motorConstraints;
}
/// <summary>
/// Builds the piston joint.
/// </summary>
/// <returns>The piston joint.</returns>
/// <param name="simulationObjs">Simulation objects.</param>
public List<JacobianContact> BuildJacobian(
SimulationObject[] simulationObjs,
double? baumStabilization = null)
{
var pistonConstraints = new List<JacobianContact> ();
SimulationObject simulationObjectA = simulationObjs [IndexA];
SimulationObject simulationObjectB = simulationObjs [IndexB];
AnchorPoint = (simulationObjectA.RotationMatrix *
(StartAnchorPoint -
simulationObjectA.StartPosition)) +
simulationObjectA.Position;
#region Init Linear
Vector3 sliderAxis = simulationObjectA.RotationMatrix * PistonAxis;
Vector3 t1 = GeometryUtilities.GetPerpendicularVector (sliderAxis).Normalize ();
Vector3 t2 = Vector3.Cross (sliderAxis, t1).Normalize ();
Vector3 r1 = simulationObjectA.RotationMatrix *
StartErrorAxis1;
Vector3 r2 = simulationObjectB.RotationMatrix *
StartErrorAxis2;
Vector3 p1 = simulationObjectA.Position + r1;
Vector3 p2 = simulationObjectB.Position + r2;
Vector3 linearError = p2 - p1;
#endregion
Vector3 angularError = sliderAxis.Cross (
(simulationObjectB.RotationMatrix * PistonAxis));
#region Jacobian Constraint
#region Base Constraints
ConstraintType constraintType = ConstraintType.Joint;
if (SpringCoefficient > 0)
constraintType = ConstraintType.SoftJoint;
//DOF 1
double angularLimit = RestoreCoefficient *
t1.Dot (angularError);
pistonConstraints.Add (
JacobianCommon.GetDOF (
IndexA,
IndexB,
new Vector3(),
new Vector3(),
1.0 * t1,
-1.0 * t1,
simulationObjectA,
simulationObjectB,
0.0,
angularLimit,
SpringCoefficient,
0.0,
constraintType));
//DOF 2
angularLimit = RestoreCoefficient *
t2.Dot (angularError);
pistonConstraints.Add (
JacobianCommon.GetDOF (
IndexA,
IndexB,
new Vector3(),
new Vector3(),
1.0 * t2,
-1.0 * t2,
simulationObjectA,
simulationObjectB,
0.0,
angularLimit,
SpringCoefficient,
0.0,
constraintType));
//DOF 3
double constraintLimit = RestoreCoefficient * Vector3.Dot (t1,linearError);
pistonConstraints.Add (JacobianCommon.GetDOF (
IndexA,
IndexB,
t1,
-1.0 * t1,
Vector3.Cross (r1, t1),
-1.0 * Vector3.Cross (r2, t1),
simulationObjectA,
simulationObjectB,
0.0,
constraintLimit,
SpringCoefficient,
0.0,
constraintType));
//DOF 4
constraintLimit = RestoreCoefficient * Vector3.Dot (t2,linearError);
pistonConstraints.Add (JacobianCommon.GetDOF (
IndexA,
IndexB,
t2,
-1.0 * t2,
Vector3.Cross (r1, t2),
-1.0 * Vector3.Cross (r2, t2),
simulationObjectA,
simulationObjectB,
0.0,
constraintLimit,
SpringCoefficient,
0.0,
constraintType));
#endregion
#region Limit Constraints
pistonConstraints.AddRange(getLinearLimit(
simulationObjectA,
simulationObjectB,
sliderAxis,
r1,
r2));
pistonConstraints.AddRange(getAnguarLimit(
simulationObjectA,
simulationObjectB,
sliderAxis));
#endregion
#region Motor Constraint
pistonConstraints.AddRange(getMotorConstraint(
simulationObjectA,
simulationObjectB,
sliderAxis));
#endregion
#endregion
return pistonConstraints;
}
/// <summary>
/// Detects the collision.
/// </summary>
/// <returns>The collision.</returns>
/// <param name="objectA">Object a.</param>
/// <param name="objectB">Object b.</param>
public GJKOutput Execute(
SimulationObject objectA,
SimulationObject objectB,
int geometryIndexA,
int geometryIndexB)
{
var collisionPoint = new CollisionPoint();
var collisionNormal = new Vector3();
var supportTriangles = new List<SupportTriangle>();
var centroid = new Vector3();
bool isIntersection = false;
double collisionDistance = ExecuteGJKAlgorithm (
objectA,
objectB,
geometryIndexA,
geometryIndexB,
ref collisionNormal,
ref collisionPoint,
ref supportTriangles,
ref centroid,
ref isIntersection);
return new GJKOutput (
collisionDistance,
collisionPoint,
collisionNormal,
centroid,
isIntersection,
supportTriangles);
}
