예제 #1
0
        public static Vector3d GetFixedAngularError(
            SoftShapePoint objectA,
            SoftShapePoint objectB,
            Quaternion relativeOrientation)
        {
            Quaternion currentRelativeOrientation = objectB.RotationStatus.Inverse() *
                                                    objectA.RotationStatus;

            Quaternion relativeOrientationError = relativeOrientation.Inverse() *
                                                  currentRelativeOrientation;

            var angularError = new Vector3d(
                relativeOrientationError.b,
                relativeOrientationError.c,
                relativeOrientationError.d);

            if (relativeOrientationError.a < 0.0)
            {
                angularError = new Vector3d(
                    -angularError.x,
                    -angularError.y,
                    -angularError.z);
            }

            return(objectA.RotationMatrix * angularError);
        }
예제 #2
0
        public static JacobianConstraint GetDOF(
            Vector3d angularComponentA,
            Vector3d angularComponentB,
            SoftShapePoint softShapePointA,
            SoftShapePoint softShapePointB,
            double constraintValue,
            double correctionValue,
            double cfm,
            double constraintLimit,
            ConstraintType type)
        {
            double jacobianVelocityValue = angularComponentA.Dot(softShapePointA.AngularVelocity) +
                                           angularComponentB.Dot(softShapePointB.AngularVelocity);

            return(new JacobianConstraint(
                       softShapePointA,
                       softShapePointB,
                       angularComponentA,
                       angularComponentB,
                       type,
                       constraintValue,
                       jacobianVelocityValue,
                       correctionValue,
                       cfm,
                       constraintLimit));
        }
예제 #3
0
 public SoftConstraint(
     SoftShapePoint pointA,
     SoftShapePoint pointB,
     ISoftShape shape,
     double errorReductionParam,
     double springCoefficient)
     : this(pointA, pointB, shape, errorReductionParam, springCoefficient, 0.0, 0.0)
 {
 }
예제 #4
0
        private void UpdatePointLinearVelocity(
            SoftShapePoint point,
            double timeStep)
        {
            var externalForce = point.ForceValue * point.MassInfo.InverseMass +
                                timeStep * EngineParameters.ExternalForce;

            point.SetLinearVelocity(point.LinearVelocity + externalForce);
        }
예제 #5
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);
        }
예제 #6
0
        private static double GetSoftBodyPointDistanceSum(
            Vector3d collisionPoint,
            ISoftShape softShape,
            int?[] linkedID)
        {
            double distanceSum = 0.0;

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

                if (softShapePoint == null)
                {
                    continue;
                }

                distanceSum += Vector3d.Length(softShapePoint.Position - collisionPoint);
            }

            return(distanceSum);
        }
예제 #7
0
        public SoftConstraint(
            SoftShapePoint pointA,
            SoftShapePoint pointB,
            ISoftShape shape,
            double errorReductionParam,
            double springCoefficient,
            double angularErrorReductionParam,
            double angularSpringCoeff)
        {
            PointA                     = pointA;
            PointB                     = pointB;
            KeyIndex                   = GetHashCode();
            SpringCoeff                = springCoefficient;
            AngularSpringCoeff         = angularSpringCoeff;
            ErrorReductionParam        = errorReductionParam;
            AngularErrorReductionParam = angularErrorReductionParam;
            Shape = shape;
            ActivateAngularConstraint = false;

            if (angularErrorReductionParam != 0.0 &&
                angularSpringCoeff != 0.0)
            {
                ActivateAngularConstraint = true;
            }

            StartAnchorPoint = (PointB.StartPosition + PointA.StartPosition) * 0.5;

            Vector3d relativePos = PointA.RotationMatrix * (StartAnchorPoint - PointA.StartPosition);

            AnchorPoint = relativePos + PointA.Position;

            StartErrorAxis1 = PointA.RotationMatrix.Transpose() *
                              (AnchorPoint - PointA.Position);

            StartErrorAxis2 = PointB.RotationMatrix.Transpose() *
                              (AnchorPoint - PointB.Position);

            RelativeOrientation = pointB.RotationStatus.Inverse() *
                                  pointA.RotationStatus;
        }
예제 #8
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);
        }