Exemplo n.º 1
0
        private JacobianConstraint[] AddFrictionConstraints(
            IShape objectA,
            IShape objectB,
            PhysicsEngineParameters simulationParameters,
            Vector3d normal,
            Vector3d relativeVelocity,
            Vector3d ra,
            Vector3d rb,
            SoftShapePoint softShapePoint)
        {
            JacobianConstraint[] friction = new JacobianConstraint[simulationParameters.FrictionDirections];

            Vector3d[] frictionDirection = GetFrictionCone(normal, simulationParameters.FrictionDirections);

            double constraintLimit = 0.0;

            Vector3d tangentialVelocity = relativeVelocity -
                                          normal.Dot(relativeVelocity) *
                                          normal;

            if (Vector3d.Length(tangentialVelocity) > simulationParameters.ShiftToStaticFrictionTolerance)
            {
                constraintLimit = 0.5 * (objectA.DynamicFrictionCoeff + objectB.DynamicFrictionCoeff);
            }
            else
            {
                constraintLimit = 0.5 * (objectA.StaticFrictionCoeff + objectB.StaticFrictionCoeff);
            }

            for (int i = 0; i < simulationParameters.FrictionDirections; i++)
            {
                var linearComponentA = frictionDirection[i];
                var linearComponentB = -1.0 * linearComponentA;

                var angularComponentA = ra.Cross(linearComponentA);
                var angularComponentB = -1.0 * rb.Cross(linearComponentA);

                friction[i] = JacobianCommon.GetDOF(
                    linearComponentA,
                    linearComponentB,
                    angularComponentA,
                    angularComponentB,
                    (objectA is ISoftShape) ? (IShapeCommon)softShapePoint: objectA,
                    (objectB is ISoftShape) ? (IShapeCommon)softShapePoint: objectB,
                    0.0,
                    0.0,
                    simulationParameters.FrictionCFM,
                    constraintLimit,
                    ConstraintType.Friction,
                    null);
            }

            return(friction);
        }
        private List <JacobianConstraint> GetMotorConstraint(
            IShape simulationObjectA,
            IShape simulationObjectB,
            Vector3d hingeAxis,
            Vector3d rotationAxis)
        {
            var motorConstraint = new List <JacobianConstraint>();

            if (SpeedHingeAxisLimit.HasValue &&
                ForceHingeAxisLimit.HasValue)
            {
                motorConstraint.Add(
                    JacobianCommon.GetDOF(
                        -1.0 * hingeAxis,
                        hingeAxis,
                        simulationObjectA,
                        simulationObjectB,
                        SpeedHingeAxisLimit.Value,
                        0.0,
                        0.0,
                        ForceHingeAxisLimit.Value,
                        ConstraintType.JointMotor));
            }

            if (SpeedRotationAxisLimit.HasValue &&
                ForceRotationAxisLimit.HasValue)
            {
                motorConstraint.Add(
                    JacobianCommon.GetDOF(
                        -1.0 * rotationAxis,
                        rotationAxis,
                        simulationObjectA,
                        simulationObjectB,
                        SpeedRotationAxisLimit.Value,
                        0.0,
                        0.0,
                        ForceRotationAxisLimit.Value,
                        ConstraintType.JointMotor));
            }

            return(motorConstraint);
        }
Exemplo n.º 3
0
        List <JacobianConstraint> GetMotorConstraint(
            IShape simulationObjectA,
            IShape simulationObjectB,
            Vector3d sliderAxis)
        {
            var motorConstraints = new List <JacobianConstraint>();

            if (LinearForceLimit.HasValue &&
                LinearSpeedValue.HasValue)
            {
                motorConstraints.Add(JacobianCommon.GetDOF(
                                         sliderAxis,
                                         -1.0 * sliderAxis,
                                         new Vector3d(),
                                         new Vector3d(),
                                         simulationObjectA,
                                         simulationObjectB,
                                         LinearSpeedValue.Value,
                                         0.0,
                                         0.0,
                                         LinearForceLimit.Value,
                                         ConstraintType.JointMotor));
            }

            if (AngularForceLimit.HasValue &&
                AngularSpeedValue.HasValue)
            {
                motorConstraints.Add(JacobianCommon.GetDOF(
                                         sliderAxis,
                                         -1.0 * sliderAxis,
                                         simulationObjectA,
                                         simulationObjectB,
                                         AngularSpeedValue.Value,
                                         0.0,
                                         0.0,
                                         AngularForceLimit.Value,
                                         ConstraintType.JointMotor));
            }

            return(motorConstraints);
        }
        private List <JacobianConstraint> GetSyncConstraintsExternalShape(
            Vector3d hingeAxis,
            Vector3d rotationAxis)
        {
            var syncConstraints = new List <JacobianConstraint>();

            if (ExternalSyncShape != null)
            {
                var rotationAxisExt = ExternalSyncShape.RotationMatrix * RotationAxis;
                var hingeAxisExt    = hingeAxis;

                syncConstraints.Add(JacobianCommon.GetDOF(
                                        -1.0 * rotationAxisExt,
                                        rotationAxis,
                                        ExternalSyncShape,
                                        ShapeB,
                                        0.0,
                                        0.0,
                                        SpringCoefficientHingeAxis,
                                        0.0,
                                        ConstraintType.Joint));

                syncConstraints.Add(JacobianCommon.GetDOF(
                                        -1.0 * hingeAxisExt,
                                        hingeAxis,
                                        ExternalSyncShape,
                                        ShapeB,
                                        0.0,
                                        0.0,
                                        SpringCoefficientHingeAxis,
                                        0.0,
                                        ConstraintType.Joint));
            }

            return(syncConstraints);
        }
Exemplo n.º 5
0
        /// <summary>
        /// Builds the ball socket joint.
        /// </summary>
        /// <returns>The ball socket joint.</returns>
        /// <param name="simulationObjs">Simulation objects.</param>
        public override List <JacobianConstraint> BuildJacobian()
        {
            var ballSocketConstraints = new List <JacobianConstraint>();

            IShape simulationObjectA = ShapeA;
            IShape simulationObjectB = ShapeB;

            #region Init Linear

            Vector3d r1 = simulationObjectA.RotationMatrix *
                          StartErrorAxis1;

            Vector3d r2 = simulationObjectB.RotationMatrix *
                          StartErrorAxis2;

            Matrix3x3 skewR1 = r1.GetSkewSymmetricMatrix();
            Matrix3x3 skewR2 = r2.GetSkewSymmetricMatrix();

            Vector3d p1 = simulationObjectA.Position + r1;
            Vector3d p2 = simulationObjectB.Position + r2;

            Vector3d linearError = p2 - p1;

            #endregion

            #region Jacobian Constraint

            double errorReduction    = ErrorReductionParam;
            double springCoefficient = SpringCoefficient;

            ConstraintType constraintType = ConstraintType.Joint;

            if (SpringCoefficient > 0)
            {
                constraintType = ConstraintType.SoftJoint;
            }

            double restoreCoeff = errorReduction;

            double constraintLimit = restoreCoeff * linearError.x;

            //DOF 1

            ballSocketConstraints.Add(JacobianCommon.GetDOF(
                                          xVec,
                                          xVecNeg,
                                          new Vector3d(-skewR1.r1c1, -skewR1.r1c2, -skewR1.r1c3),
                                          new Vector3d(skewR2.r1c1, skewR2.r1c2, skewR2.r1c3),
                                          simulationObjectA,
                                          simulationObjectB,
                                          0.0,
                                          constraintLimit,
                                          springCoefficient,
                                          0.0,
                                          constraintType));

            //DOF 2

            constraintLimit = restoreCoeff * linearError.y;

            ballSocketConstraints.Add(JacobianCommon.GetDOF(
                                          yVec,
                                          yVecNeg,
                                          new Vector3d(-skewR1.r2c1, -skewR1.r2c2, -skewR1.r2c3),
                                          new Vector3d(skewR2.r2c1, skewR2.r2c2, skewR2.r2c3),
                                          simulationObjectA,
                                          simulationObjectB,
                                          0.0,
                                          constraintLimit,
                                          springCoefficient,
                                          0.0,
                                          constraintType));

            //DOF 3

            constraintLimit = restoreCoeff * linearError.z;

            ballSocketConstraints.Add(JacobianCommon.GetDOF(
                                          zVec,
                                          zVecNeg,
                                          new Vector3d(-skewR1.r3c1, -skewR1.r3c2, -skewR1.r3c3),
                                          new Vector3d(skewR2.r3c1, skewR2.r3c2, skewR2.r3c3),
                                          simulationObjectA,
                                          simulationObjectB,
                                          0.0,
                                          constraintLimit,
                                          springCoefficient,
                                          0.0,
                                          constraintType));

            #endregion

            return(ballSocketConstraints);
        }
Exemplo n.º 6
0
        private List <JacobianConstraint> BuildSoftBodyVSRigidBodyCollisionConstraints(
            CollisionPointStructure collisionPointStr,
            ISoftShape objectA,
            IShape objectB)
        {
            List <JacobianConstraint> contactConstraints = new List <JacobianConstraint>();

            IShape iSoftShape = (IShape)objectA;

            double restitutionCoeff = GetRestitutionCoeff(iSoftShape, objectB);

            double baumgarteStabValue = GetBaumgarteStabilizationValue(iSoftShape, objectB);

            for (int h = 0; h < collisionPointStr.CollisionPointBase.Length; h++)
            {
                int?[] linkedID = collisionPointStr.CollisionPointBase[h].CollisionPoint.CollisionPointA.LinkedID.Distinct().ToArray();

                double distanceSum = GetSoftBodyPointDistanceSum(
                    collisionPointStr.CollisionPointBase[h].CollisionPoint.CollisionPointA.Vertex,
                    objectA,
                    linkedID);

                for (int i = 0; i < linkedID.Length; i++)
                {
                    SoftShapePoint softShapePoint = objectA.ShapePoints.FirstOrDefault(x => x.ID == linkedID[i]);

                    if (softShapePoint == null)
                    {
                        continue;
                    }

                    double distanceWeigth = 1.0;

                    if (distanceSum > 0.0)
                    {
                        distanceWeigth = Vector3d.Length(softShapePoint.Position - collisionPointStr.CollisionPointBase[h].CollisionPoint.CollisionPointA.Vertex) / distanceSum;
                    }
                    if (distanceWeigth < 1E-10)
                    {
                        continue;
                    }

                    Vector3d ra = Vector3d.Zero();
                    Vector3d rb = collisionPointStr.CollisionPointBase[h].CollisionPoint.CollisionPointB.Vertex - objectB.Position;

                    ////Component
                    Vector3d linearComponentA = (-1.0 * collisionPointStr.CollisionPointBase[h].CollisionPoint.CollisionNormal).Normalize();
                    Vector3d linearComponentB = -1.0 * linearComponentA;

                    Vector3d angularComponentA = Vector3d.Zero();
                    Vector3d angularComponentB = -1.0 * rb.Cross(linearComponentA);

                    ////Velocity
                    Vector3d velocityA = softShapePoint.LinearVelocity;

                    Vector3d velocityB = objectB.LinearVelocity +
                                         objectB.AngularVelocity.Cross(rb);

                    Vector3d relativeVelocity = velocityB - velocityA;



                    if (relativeVelocity.Length() < 1E-12 &&
                        collisionPointStr.CollisionPointBase[h].CollisionPoint.Intersection &&
                        collisionPointStr.CollisionPointBase[h].CollisionPoint.Distance < 1E-10)
                    {
                        continue;
                    }

                    #region Normal direction contact

                    double linearComponent = linearComponentA.Dot(relativeVelocity);

                    double uCollision = restitutionCoeff * Math.Max(0.0, linearComponent);

                    if (uCollision <= 0.0 &&
                        !collisionPointStr.CollisionPointBase[h].CollisionPoint.Intersection)
                    {
                        continue;
                    }

                    double correctionParameter = 0.0;

                    if (collisionPointStr.CollisionPointBase[h].CollisionPoint.Intersection)
                    {
                        //Limit the Baum stabilization jitter effect
                        correctionParameter = Math.Max(Math.Max(collisionPointStr.CollisionPointBase[h].CollisionPoint.Distance - simulationParameters.CompenetrationTolerance, 0.0) *
                                                       baumgarteStabValue - uCollision, 0.0);
                    }

                    double correctedBounce = uCollision * distanceWeigth;
                    correctionParameter = correctionParameter * distanceWeigth;

                    JacobianConstraint normalContact = JacobianCommon.GetDOF(
                        linearComponentA,
                        linearComponentB,
                        angularComponentA,
                        angularComponentB,
                        softShapePoint,
                        objectB,
                        correctedBounce,
                        correctionParameter,
                        simulationParameters.NormalCFM,
                        0.0,
                        ConstraintType.Collision,
                        null);

                    #endregion

                    #region Friction Contact

                    JacobianConstraint[] frictionContact =
                        AddFrictionConstraints(
                            iSoftShape,
                            objectB,
                            simulationParameters,
                            linearComponentA,
                            relativeVelocity,
                            ra,
                            rb,
                            softShapePoint);

                    #endregion

                    contactConstraints.Add(normalContact);

                    int normalIndex = contactConstraints.Count - 1;
                    foreach (JacobianConstraint fjc in frictionContact)
                    {
                        fjc.SetContactReference(normalIndex);
                        contactConstraints.Add(fjc);
                    }
                }
            }

            return(contactConstraints);
        }
Exemplo n.º 7
0
        private List <JacobianConstraint> BuildRigidBodyCollisionConstraints(
            CollisionPointStructure collisionPointStr,
            IShape objectA,
            IShape objectB)
        {
            List <JacobianConstraint> contactConstraints = new List <JacobianConstraint>();

            double restitutionCoefficient =
                (objectA.RestitutionCoeff +
                 objectB.RestitutionCoeff) * 0.5;

            double baumgarteStabilizationValue =
                (objectA.RestoreCoeff +
                 objectB.RestoreCoeff) * 0.5;

            for (int h = 0; h < collisionPointStr.CollisionPointBase.Length; h++)
            {
                var collisionPointBase = collisionPointStr.CollisionPointBase[h];

                for (int k = 0; k < collisionPointBase.CollisionPoints.Length; k++)
                {
                    CollisionPoint collisionPoint = collisionPointBase.CollisionPoints[k];

                    Vector3d ra = collisionPoint.CollisionPointA.Vertex - objectA.Position;
                    Vector3d rb = collisionPoint.CollisionPointB.Vertex - objectB.Position;

                    Vector3d linearComponentA = (-1.0 * collisionPoint.CollisionNormal).Normalize();
                    Vector3d linearComponentB = -1.0 * linearComponentA;

                    Vector3d angularComponentA = ra.Cross(linearComponentA);
                    Vector3d angularComponentB = -1.0 * rb.Cross(linearComponentA);

                    Vector3d velocityA = objectA.LinearVelocity +
                                         objectA.AngularVelocity.Cross(ra);

                    Vector3d velocityB = objectB.LinearVelocity +
                                         objectB.AngularVelocity.Cross(rb);

                    Vector3d relativeVelocity = velocityB - velocityA;

                    if (relativeVelocity.Length() < 1E-12 &&
                        collisionPointBase.CollisionPoint.Intersection &&
                        collisionPointBase.CollisionPoint.Distance < 1E-10)
                    {
                        continue;
                    }

                    #region Normal direction contact

                    double linearComponent = linearComponentA.Dot(relativeVelocity);

                    double uCollision = restitutionCoefficient * Math.Max(0.0, linearComponent);

                    if (uCollision <= 0.0 &&
                        !collisionPointBase.CollisionPoint.Intersection)
                    {
                        continue;
                    }

                    double correctionParameter = 0.0;

                    if (collisionPointBase.CollisionPoint.Intersection)
                    {
                        //Limit the Baum stabilization jitter effect
                        correctionParameter = Math.Max(Math.Max(collisionPointBase.CollisionPoint.Distance - simulationParameters.CompenetrationTolerance, 0.0) *
                                                       baumgarteStabilizationValue, 0.0);
                    }

                    double correctedBounce = uCollision;

                    JacobianConstraint normalContact = JacobianCommon.GetDOF(
                        linearComponentA,
                        linearComponentB,
                        angularComponentA,
                        angularComponentB,
                        objectA,
                        objectB,
                        correctedBounce,
                        correctionParameter,
                        simulationParameters.NormalCFM,
                        0.0,
                        ConstraintType.Collision,
                        null);

                    #endregion

                    contactConstraints.Add(normalContact);

                    #region Friction Contact

                    JacobianConstraint[] frictionContact =
                        AddFrictionConstraints(
                            objectA,
                            objectB,
                            simulationParameters,
                            linearComponentA,
                            relativeVelocity,
                            ra,
                            rb);

                    #endregion

                    int normalIndex = contactConstraints.Count - 1;
                    foreach (JacobianConstraint fjc in frictionContact)
                    {
                        fjc.SetContactReference(normalIndex);
                        contactConstraints.Add(fjc);
                    }
                }
            }

            return(contactConstraints);
        }
Exemplo n.º 8
0
        /// <summary>
        /// Builds the fixed joint.
        /// </summary>
        /// <returns>The fixed joint.</returns>
        /// <param name="simulationObjs">Simulation objects.</param>
        public List <JacobianConstraint> BuildJacobian()
        {
            var softConstraints = new List <JacobianConstraint>();

            #region Init Linear

            Vector3d r1 = PointA.RotationMatrix *
                          StartErrorAxis1;

            Vector3d r2 = PointB.RotationMatrix *
                          StartErrorAxis2;

            Vector3d p1 = PointA.Position + r1;
            Vector3d p2 = PointB.Position + r2;

            Vector3d linearError = p2 - p1;

            Matrix3x3 skewR1 = r1.GetSkewSymmetricMatrix();
            Matrix3x3 skewR2 = r2.GetSkewSymmetricMatrix();

            #endregion

            #region Init Angular

            Vector3d angularError = JacobianCommon.GetFixedAngularError(
                PointA,
                PointB,
                RelativeOrientation);

            #endregion

            #region Jacobian Constraint

            double errorReduction    = ErrorReductionParam;
            double springCoefficient = SpringCoeff;

            ConstraintType constraintType = ConstraintType.SoftJoint;

            double constraintLimit = errorReduction * linearError.x;

            //DOF 1

            softConstraints.Add(JacobianCommon.GetDOF(
                                    xVec,
                                    xVecNeg,
                                    new Vector3d(-skewR1.r1c1, -skewR1.r1c2, -skewR1.r1c3),
                                    new Vector3d(skewR2.r1c1, skewR2.r1c2, skewR2.r1c3),
                                    PointA,
                                    PointB,
                                    0.0,
                                    constraintLimit,
                                    springCoefficient,
                                    0.0,
                                    constraintType));

            //DOF 2

            constraintLimit = errorReduction * linearError.y;

            softConstraints.Add(JacobianCommon.GetDOF(
                                    yVec,
                                    yVecNeg,
                                    new Vector3d(-skewR1.r2c1, -skewR1.r2c2, -skewR1.r2c3),
                                    new Vector3d(skewR2.r2c1, skewR2.r2c2, skewR2.r2c3),
                                    PointA,
                                    PointB,
                                    0.0,
                                    constraintLimit,
                                    springCoefficient,
                                    0.0,
                                    constraintType));

            //DOF 3

            constraintLimit = errorReduction * linearError.z;

            softConstraints.Add(JacobianCommon.GetDOF(
                                    zVec,
                                    zVecNeg,
                                    new Vector3d(-skewR1.r3c1, -skewR1.r3c2, -skewR1.r3c3),
                                    new Vector3d(skewR2.r3c1, skewR2.r3c2, skewR2.r3c3),
                                    PointA,
                                    PointB,
                                    0.0,
                                    constraintLimit,
                                    springCoefficient,
                                    0.0,
                                    constraintType));

            if (ActivateAngularConstraint)
            {
                double angErrorReduction    = AngularErrorReductionParam;
                double angSpringCoefficient = AngularSpringCoeff;

                //DOF 4

                constraintLimit = angErrorReduction * angularError.x;

                softConstraints.Add(JacobianCommon.GetDOF(
                                        xVecNeg,
                                        xVec,
                                        PointA,
                                        PointB,
                                        0.0,
                                        constraintLimit,
                                        angSpringCoefficient,
                                        0.0,
                                        constraintType));

                //DOF 5

                constraintLimit = angErrorReduction * angularError.y;

                softConstraints.Add(JacobianCommon.GetDOF(
                                        yVecNeg,
                                        yVec,
                                        PointA,
                                        PointB,
                                        0.0,
                                        constraintLimit,
                                        angSpringCoefficient,
                                        0.0,
                                        constraintType));

                //DOF 6

                constraintLimit = angErrorReduction * angularError.z;

                softConstraints.Add(JacobianCommon.GetDOF(
                                        zVecNeg,
                                        zVec,
                                        PointA,
                                        PointB,
                                        0.0,
                                        constraintLimit,
                                        angSpringCoefficient,
                                        0.0,
                                        constraintType));
            }

            #endregion

            return(softConstraints);
        }
Exemplo n.º 9
0
        /// <summary>
        /// Builds the hinge joint.
        /// </summary>
        /// <returns>The hinge joint.</returns>
        /// <param name="simulationObjs">Simulation objects.</param>
        public override List <JacobianConstraint> BuildJacobian()
        {
            var hingeConstraints = new List <JacobianConstraint> ();

            IShape simulationObjectA = ShapeA;
            IShape simulationObjectB = ShapeB;

            #region Init Linear

            Vector3d axisRotated = GetHingeAxis();

            Vector3d t1 = GeometryUtils.GetPerpendicularVector(axisRotated).Normalize();
            Vector3d t2 = axisRotated.Cross(t1).Normalize();

            Vector3d r1 = simulationObjectA.RotationMatrix *
                          StartErrorAxis1;

            Vector3d r2 = simulationObjectB.RotationMatrix *
                          StartErrorAxis2;

            Matrix3x3 skewP1 = Matrix3x3.GetSkewSymmetricMatrix(r1);
            Matrix3x3 skewP2 = Matrix3x3.GetSkewSymmetricMatrix(r2);

            Vector3d p1 = simulationObjectA.Position + r1;
            Vector3d p2 = simulationObjectB.Position + r2;

            Vector3d linearError = p2 - p1;

            #endregion

            #region Init Angular

            Vector3d angularError = JacobianCommon.GetFixedAngularError(
                simulationObjectA,
                simulationObjectB,
                RelativeOrientation);

            #endregion

            #region Jacobian Constraint

            double errorReduction    = ErrorReductionParam;
            double springCoefficient = SpringCoefficient;

            #region Base Constraint

            ConstraintType constraintType = ConstraintType.Joint;

            if (SpringCoefficient > 0)
            {
                constraintType = ConstraintType.SoftJoint;
            }

            //DOF 1

            double constraintLimit = errorReduction * linearError.x;

            hingeConstraints.Add(JacobianCommon.GetDOF(
                                     xVec,
                                     xVecNeg,
                                     new Vector3d(-skewP1.r1c1, -skewP1.r1c2, -skewP1.r1c3),
                                     new Vector3d(skewP2.r1c1, skewP2.r1c2, skewP2.r1c3),
                                     simulationObjectA,
                                     simulationObjectB,
                                     0.0,
                                     constraintLimit,
                                     springCoefficient,
                                     0.0,
                                     constraintType));

            //DOF 2

            constraintLimit = errorReduction * linearError.y;

            hingeConstraints.Add(JacobianCommon.GetDOF(
                                     yVec,
                                     yVecNeg,
                                     new Vector3d(-skewP1.r2c1, -skewP1.r2c2, -skewP1.r2c3),
                                     new Vector3d(skewP2.r2c1, skewP2.r2c2, skewP2.r2c3),
                                     simulationObjectA,
                                     simulationObjectB,
                                     0.0,
                                     constraintLimit,
                                     springCoefficient,
                                     0.0,
                                     constraintType));

            //DOF 3

            constraintLimit = errorReduction * linearError.z;

            hingeConstraints.Add(JacobianCommon.GetDOF(
                                     zVec,
                                     zVecNeg,
                                     new Vector3d(-skewP1.r3c1, -skewP1.r3c2, -skewP1.r3c3),
                                     new Vector3d(skewP2.r3c1, skewP2.r3c2, skewP2.r3c3),
                                     simulationObjectA,
                                     simulationObjectB,
                                     0.0,
                                     constraintLimit,
                                     springCoefficient,
                                     0.0,
                                     constraintType));

            //DOF 4

            double angularLimit = errorReduction *
                                  t1.Dot(angularError);

            hingeConstraints.Add(
                JacobianCommon.GetDOF(
                    -1.0 * t1,
                    1.0 * t1,
                    simulationObjectA,
                    simulationObjectB,
                    0.0,
                    angularLimit,
                    springCoefficient,
                    0.0,
                    constraintType));

            //DOF 5

            angularLimit = errorReduction *
                           t2.Dot(angularError);

            hingeConstraints.Add(
                JacobianCommon.GetDOF(
                    -1.0 * t2,
                    1.0 * t2,
                    simulationObjectA,
                    simulationObjectB,
                    0.0,
                    angularLimit,
                    springCoefficient,
                    0.0,
                    constraintType));

            #endregion

            #region Limit Constraints

            if (AngularLimitMin.HasValue &&
                AngularLimitMax.HasValue)
            {
                double angle = JacobianCommon.GetAngle(
                    simulationObjectA,
                    simulationObjectB,
                    RelativeOrientation,
                    HingeAxis);

                JacobianConstraint?jContact =
                    JacobianCommon.GetAngularLimit(
                        angle,
                        errorReduction,
                        0.0,
                        simulationObjectA,
                        simulationObjectB,
                        axisRotated,
                        AngularLimitMin.Value,
                        AngularLimitMax.Value);

                if (jContact != null)
                {
                    hingeConstraints.Add(jContact.Value);
                }
            }

            #endregion

            #region Motor Contraint

            if (SpeedValue.HasValue &&
                ForceLimit.HasValue)
            {
                hingeConstraints.Add(
                    JacobianCommon.GetDOF(
                        -1.0 * axisRotated,
                        1.0 * axisRotated,
                        simulationObjectA,
                        simulationObjectB,
                        SpeedValue.Value,
                        0.0,
                        0.0,
                        ForceLimit.Value,
                        ConstraintType.JointMotor));
            }

            #endregion

            #endregion

            return(hingeConstraints);
        }
Exemplo n.º 10
0
        public override List <JacobianConstraint> BuildJacobian()
        {
            var angularConstraints = new List <JacobianConstraint>();

            IShape simulationObjectA = ShapeA;
            IShape simulationObjectB = ShapeB;

            double errorReduction                = ErrorReductionParam;
            double springCoefficientHingeAxis    = SpringCoefficientHingeAxis;
            double springCoefficientRotationAxis = SpringCoefficientRotationAxis;

            #region Init Angular

            Vector3d hingeAxis    = GetHingeAxis();
            Vector3d rotationAxis = GetRotationAxis();

            double   k = hingeAxis.Dot(rotationAxis);
            Vector3d tempPerpendicular = rotationAxis - k * hingeAxis;
            Vector3d t1 = hingeAxis.Cross(tempPerpendicular).Normalize();

            double hingeAngle = GetAngle1(
                hingeAxis,
                rotationAxis,
                HingeAxis,
                simulationObjectA.RotationStatus,
                RelativeOrientation1);

            double twistAngle = GetAngle2(
                hingeAxis,
                rotationAxis,
                RotationAxis,
                simulationObjectB.RotationStatus,
                RelativeOrientation2);

            #endregion

            #region Jacobian Constraint

            double angularLimit = errorReduction * hingeAngle;

            angularConstraints.Add(JacobianCommon.GetDOF(
                                       hingeAxis,
                                       -1.0 * hingeAxis,
                                       simulationObjectA,
                                       simulationObjectB,
                                       0.0,
                                       angularLimit,
                                       springCoefficientHingeAxis,
                                       0.0,
                                       ConstraintType.Joint));

            angularLimit = errorReduction * twistAngle;

            angularConstraints.Add(JacobianCommon.GetDOF(
                                       rotationAxis,
                                       -1.0 * rotationAxis,
                                       simulationObjectA,
                                       simulationObjectB,
                                       0.0,
                                       angularLimit,
                                       springCoefficientRotationAxis,
                                       0.0,
                                       ConstraintType.Joint));

            #endregion

            return(angularConstraints);
        }
Exemplo n.º 11
0
        /// <summary>
        /// Builds the piston joint.
        /// </summary>
        /// <returns>The piston joint.</returns>
        /// <param name="simulationObjs">Simulation objects.</param>
        public override List <JacobianConstraint> BuildJacobian()
        {
            var pistonConstraints = new List <JacobianConstraint> ();

            IShape simulationObjectA = ShapeA;
            IShape simulationObjectB = ShapeB;

            #region Init Linear

            Vector3d sliderAxis = GetSliderAxis();

            Vector3d t1 = GeometryUtils.GetPerpendicularVector(sliderAxis).Normalize();
            Vector3d t2 = sliderAxis.Cross(t1).Normalize();

            Vector3d r1 = simulationObjectA.RotationMatrix *
                          StartErrorAxis1;

            Vector3d r2 = simulationObjectB.RotationMatrix *
                          StartErrorAxis2;

            Vector3d p1 = simulationObjectA.Position + r1;
            Vector3d p2 = simulationObjectB.Position + r2;

            Vector3d linearError = p2 - p1;

            #endregion

            Vector3d angularError = sliderAxis.Cross(
                (simulationObjectB.RotationMatrix * PistonAxis));

            #region Jacobian Constraint

            double errorReduction    = ErrorReductionParam;
            double springCoefficient = SpringCoefficient;

            #region Base Constraints

            ConstraintType constraintType = ConstraintType.Joint;

            if (SpringCoefficient > 0)
            {
                constraintType = ConstraintType.SoftJoint;
            }

            //DOF 1

            double angularLimit = errorReduction *
                                  t1.Dot(angularError);

            pistonConstraints.Add(
                JacobianCommon.GetDOF(
                    t1,
                    -1.0 * t1,
                    simulationObjectA,
                    simulationObjectB,
                    0.0,
                    angularLimit,
                    springCoefficient,
                    0.0,
                    constraintType));

            //DOF 2

            angularLimit = errorReduction *
                           t2.Dot(angularError);

            pistonConstraints.Add(
                JacobianCommon.GetDOF(
                    t2,
                    -1.0 * t2,
                    simulationObjectA,
                    simulationObjectB,
                    0.0,
                    angularLimit,
                    springCoefficient,
                    0.0,
                    constraintType));

            //DOF 3

            double constraintLimit = errorReduction * t1.Dot(linearError);

            pistonConstraints.Add(JacobianCommon.GetDOF(
                                      t1,
                                      -1.0 * t1,
                                      r1.Cross(t1),
                                      -1.0 * r2.Cross(t1),
                                      simulationObjectA,
                                      simulationObjectB,
                                      0.0,
                                      constraintLimit,
                                      springCoefficient,
                                      0.0,
                                      constraintType));

            //DOF 4

            constraintLimit = errorReduction * Vector3d.Dot(t2, linearError);

            pistonConstraints.Add(JacobianCommon.GetDOF(
                                      t2,
                                      -1.0 * t2,
                                      r1.Cross(t2),
                                      -1.0 * r2.Cross(t2),
                                      simulationObjectA,
                                      simulationObjectB,
                                      0.0,
                                      constraintLimit,
                                      springCoefficient,
                                      0.0,
                                      constraintType));

            #endregion

            #region Limit Constraints

            pistonConstraints.AddRange(GetLinearLimit(
                                           simulationObjectA,
                                           simulationObjectB,
                                           sliderAxis,
                                           r1,
                                           r2,
                                           errorReduction));

            pistonConstraints.AddRange(GetAnguarLimit(
                                           simulationObjectA,
                                           simulationObjectB,
                                           sliderAxis,
                                           errorReduction));

            #endregion

            #region Motor Constraint

            pistonConstraints.AddRange(GetMotorConstraint(
                                           simulationObjectA,
                                           simulationObjectB,
                                           sliderAxis));

            #endregion

            #endregion

            return(pistonConstraints);
        }
Exemplo n.º 12
0
        /// <summary>
        /// Builds the slider joint.
        /// </summary>
        /// <returns>The slider joint.</returns>
        /// <param name="simulationObjs">Simulation objects.</param>
        public override List <JacobianConstraint> BuildJacobian()
        {
            var sliderConstraints = new List <JacobianConstraint> ();

            IShape simulationObjectA = ShapeA;
            IShape simulationObjectB = ShapeB;

            #region Init Linear

            Vector3d sliderAxis = GetSliderAxis();

            Vector3d t1 = GeometryUtils.GetPerpendicularVector(sliderAxis).Normalize();
            Vector3d t2 = sliderAxis.Cross(t1).Normalize();

            Vector3d r1 = simulationObjectA.RotationMatrix *
                          StartErrorAxis1;

            Vector3d r2 = simulationObjectB.RotationMatrix *
                          StartErrorAxis2;

            Vector3d p1 = simulationObjectA.Position + r1;
            Vector3d p2 = simulationObjectB.Position + r2;

            Vector3d linearError = p2 - p1;

            #endregion

            #region Init Angular

            Vector3d angularError = JacobianCommon.GetFixedAngularError(
                simulationObjectA,
                simulationObjectB,
                RelativeOrientation);

            #endregion

            #region Jacobian Constraint

            double errorReduction    = ErrorReductionParam;
            double springCoefficient = SpringCoefficient;

            #region Base Constraints

            ConstraintType constraintType = ConstraintType.Joint;

            if (SpringCoefficient > 0)
            {
                constraintType = ConstraintType.SoftJoint;
            }

            double constraintLimit = errorReduction * 2.0 * angularError.x;

            //DOF 1

            sliderConstraints.Add(JacobianCommon.GetDOF(
                                      xVecNeg,
                                      xVec,
                                      simulationObjectA,
                                      simulationObjectB,
                                      0.0,
                                      constraintLimit,
                                      springCoefficient,
                                      0.0,
                                      constraintType));

            //DOF 2

            constraintLimit = errorReduction * 2.0 * angularError.y;

            sliderConstraints.Add(JacobianCommon.GetDOF(
                                      yVecNeg,
                                      yVec,
                                      simulationObjectA,
                                      simulationObjectB,
                                      0.0,
                                      constraintLimit,
                                      springCoefficient,
                                      0.0,
                                      constraintType));

            //DOF 3

            constraintLimit = errorReduction * 2.0 * angularError.z;

            sliderConstraints.Add(JacobianCommon.GetDOF(
                                      zVecNeg,
                                      zVec,
                                      simulationObjectA,
                                      simulationObjectB,
                                      0.0,
                                      constraintLimit,
                                      springCoefficient,
                                      0.0,
                                      constraintType));

            //DOF 4

            constraintLimit = errorReduction * t1.Dot(linearError);

            sliderConstraints.Add(JacobianCommon.GetDOF(
                                      t1,
                                      -1.0 * t1,
                                      r1.Cross(t1),
                                      -1.0 * r2.Cross(t1),
                                      simulationObjectA,
                                      simulationObjectB,
                                      0.0,
                                      constraintLimit,
                                      springCoefficient,
                                      0.0,
                                      constraintType));

            //DOF 5

            constraintLimit = errorReduction * t2.Dot(linearError);

            sliderConstraints.Add(JacobianCommon.GetDOF(
                                      t2,
                                      -1.0 * t2,
                                      r1.Cross(t2),
                                      -1.0 * r2.Cross(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(
                        simulationObjectA,
                        simulationObjectB,
                        sliderAxis,
                        r1,
                        r2,
                        errorReduction,
                        0.0,
                        LinearLimitMin.Value,
                        LinearLimitMax.Value));
            }

            #endregion

            #region Motor Constraint

            if (ForceLimit.HasValue &&
                SpeedValue.HasValue)
            {
                sliderConstraints.Add(JacobianCommon.GetDOF(
                                          sliderAxis,
                                          -1.0 * sliderAxis,
                                          new Vector3d(),
                                          new Vector3d(),
                                          simulationObjectA,
                                          simulationObjectB,
                                          SpeedValue.Value,
                                          0.0,
                                          0.0,
                                          ForceLimit.Value,
                                          ConstraintType.JointMotor));
            }

            #endregion

            #endregion

            return(sliderConstraints);
        }
Exemplo n.º 13
0
        /// <summary>
        /// Builds the Universal joint.
        /// </summary>
        /// <returns>The Universal joint.</returns>
        /// <param name="simulationObjs">Simulation objects.</param>
        public override List <JacobianConstraint> BuildJacobian()
        {
            var hinge2Constraints = new List <JacobianConstraint> ();

            IShape simulationObjectA = ShapeA;
            IShape simulationObjectB = ShapeB;

            #region Init Linear

            Vector3d r1 = simulationObjectA.RotationMatrix *
                          StartErrorAxis1;

            Vector3d r2 = simulationObjectB.RotationMatrix *
                          StartErrorAxis2;

            Vector3d p1 = simulationObjectA.Position + r1;
            Vector3d p2 = simulationObjectB.Position + r2;

            Vector3d linearError = p2 - p1;

            #endregion

            #region Init Angular

            Vector3d hingeAxis    = GetHingeAxis();
            Vector3d rotationAxis = GetRotationAxis();

            double   k = hingeAxis.Dot(rotationAxis);
            Vector3d tempPerpendicular = rotationAxis - k * hingeAxis;
            Vector3d t1 = hingeAxis.Cross(tempPerpendicular).Normalize();

            #endregion

            #region Jacobian Constraint

            double errorReduction    = ErrorReductionParam;
            double springCoefficient = SpringCoefficient;

            #region Base Constraint

            ConstraintType constraintType = ConstraintType.Joint;

            if (SpringCoefficient > 0.0)
            {
                constraintType = ConstraintType.SoftJoint;
            }

            //DOF 1

            double constraintLimit = errorReduction * t1.Dot(linearError);

            hinge2Constraints.Add(JacobianCommon.GetDOF(
                                      t1,
                                      -1.0 * t1,
                                      r1.Cross(t1),
                                      -1.0 * r2.Cross(t1),
                                      simulationObjectA,
                                      simulationObjectB,
                                      0.0,
                                      constraintLimit,
                                      springCoefficient,
                                      0.0,
                                      constraintType));

            //DOF 2

            constraintLimit = errorReduction * Vector3d.Dot(tempPerpendicular, linearError);

            hinge2Constraints.Add(JacobianCommon.GetDOF(
                                      tempPerpendicular,
                                      -1.0 * tempPerpendicular,
                                      r1.Cross(tempPerpendicular),
                                      -1.0 * r2.Cross(tempPerpendicular),
                                      simulationObjectA,
                                      simulationObjectB,
                                      0.0,
                                      constraintLimit,
                                      springCoefficient,
                                      0.0,
                                      constraintType));

            //DOF 3

            ConstraintType hingeAxisConstraintType = ConstraintType.Joint;
            if (SpringCoefficientHingeAxis > 0)
            {
                hingeAxisConstraintType = ConstraintType.SoftJoint;
            }

            constraintLimit = errorReduction * hingeAxis.Dot(linearError);

            hinge2Constraints.Add(JacobianCommon.GetDOF(
                                      hingeAxis,
                                      -1.0 * hingeAxis,
                                      r1.Cross(hingeAxis),
                                      -1.0 * r2.Cross(hingeAxis),
                                      simulationObjectA,
                                      simulationObjectB,
                                      0.0,
                                      constraintLimit,
                                      SpringCoefficientHingeAxis,
                                      0.0,
                                      hingeAxisConstraintType));

            //DOF 4

            double angularLimit = errorReduction * (-k);

            hinge2Constraints.Add(
                JacobianCommon.GetDOF(
                    t1,
                    -1.0 * t1,
                    simulationObjectA,
                    simulationObjectB,
                    0.0,
                    angularLimit,
                    springCoefficient,
                    0.0,
                    constraintType));

            hinge2Constraints.AddRange(GetSyncConstraintsExternalShape(
                                           hingeAxis,
                                           rotationAxis));

            #endregion

            #region Limit Constraints

            hinge2Constraints.AddRange(GetAngularLimit(
                                           simulationObjectA,
                                           simulationObjectB,
                                           hingeAxis,
                                           rotationAxis,
                                           errorReduction));

            #endregion

            #region Motor Constraint

            hinge2Constraints.AddRange(GetMotorConstraint(
                                           simulationObjectA,
                                           simulationObjectB,
                                           hingeAxis,
                                           rotationAxis));

            #endregion

            #endregion

            return(hinge2Constraints);
        }
Exemplo n.º 14
0
        /// <summary>
        /// Builds the Universal joint.
        /// </summary>
        /// <returns>The Universal joint.</returns>
        /// <param name="simulationObjs">Simulation objects.</param>
        public override List <JacobianConstraint> BuildJacobian()
        {
            var universalConstraints = new List <JacobianConstraint> ();

            IShape simulationObjectA = ShapeA;
            IShape simulationObjectB = ShapeB;

            AnchorPoint = (simulationObjectA.RotationMatrix *
                           (StartAnchorPoint - simulationObjectA.InitCenterOfMass)) +
                          simulationObjectA.Position;

            #region Init Linear

            Vector3d r1 = simulationObjectA.RotationMatrix *
                          StartErrorAxis1;

            Vector3d r2 = simulationObjectB.RotationMatrix *
                          StartErrorAxis2;

            Matrix3x3 skewP1 = Matrix3x3.GetSkewSymmetricMatrix(r1);
            Matrix3x3 skewP2 = Matrix3x3.GetSkewSymmetricMatrix(r2);

            Vector3d p1 = simulationObjectA.Position + r1;
            Vector3d p2 = simulationObjectB.Position + r2;

            Vector3d linearError = p2 - p1;

            #endregion

            #region Init Angular

            Vector3d hingeAxis    = simulationObjectA.RotationMatrix * HingeAxis;
            Vector3d rotationAxis = simulationObjectB.RotationMatrix * RotationAxis;

            double   k = hingeAxis.Dot(rotationAxis);
            Vector3d tempPerpendicular = rotationAxis - k * hingeAxis;
            Vector3d t1 = hingeAxis.Cross(tempPerpendicular).Normalize();

            #endregion

            #region Jacobian Constraint

            #region Base Constraint

            double errorReduction    = ErrorReductionParam;
            double springCoefficient = SpringCoefficient;

            ConstraintType constraintType = ConstraintType.Joint;

            if (SpringCoefficient > 0)
            {
                constraintType = ConstraintType.SoftJoint;
            }

            //DOF 1

            double constraintLimit = errorReduction * linearError.x;

            universalConstraints.Add(JacobianCommon.GetDOF(
                                         xVec,
                                         xVecNeg,
                                         new Vector3d(-skewP1.r1c1, -skewP1.r1c2, -skewP1.r1c3),
                                         new Vector3d(skewP2.r1c1, skewP2.r1c2, skewP2.r1c3),
                                         simulationObjectA,
                                         simulationObjectB,
                                         0.0,
                                         constraintLimit,
                                         springCoefficient,
                                         0.0,
                                         constraintType));

            //DOF 2

            constraintLimit = errorReduction * linearError.y;

            universalConstraints.Add(JacobianCommon.GetDOF(
                                         yVec,
                                         yVecNeg,
                                         new Vector3d(-skewP1.r2c1, -skewP1.r2c2, -skewP1.r2c3),
                                         new Vector3d(skewP2.r2c1, skewP2.r2c2, skewP2.r2c3),
                                         simulationObjectA,
                                         simulationObjectB,
                                         0.0,
                                         constraintLimit,
                                         springCoefficient,
                                         0.0,
                                         constraintType));

            //DOF 3

            constraintLimit = errorReduction * linearError.z;

            universalConstraints.Add(JacobianCommon.GetDOF(
                                         zVec,
                                         zVecNeg,
                                         new Vector3d(-skewP1.r3c1, -skewP1.r3c2, -skewP1.r3c3),
                                         new Vector3d(skewP2.r3c1, skewP2.r3c2, skewP2.r3c3),
                                         simulationObjectA,
                                         simulationObjectB,
                                         0.0,
                                         constraintLimit,
                                         springCoefficient,
                                         0.0,
                                         constraintType));

            //DOF 4

            double angularLimit = errorReduction * (-k);

            universalConstraints.Add(
                JacobianCommon.GetDOF(
                    t1,
                    -1.0 * t1,
                    simulationObjectA,
                    simulationObjectB,
                    0.0,
                    angularLimit,
                    springCoefficient,
                    0.0,
                    constraintType));

            #endregion

            #region Limit Constraints

            universalConstraints.AddRange(GetAngularLimit(
                                              simulationObjectA,
                                              simulationObjectB,
                                              hingeAxis,
                                              rotationAxis,
                                              errorReduction));

            #endregion

            #endregion

            return(universalConstraints);
        }