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