Exemplo n.º 1
0
        ///bilateral constraint between two dynamic objects
        ///positive distance = separation, negative distance = penetration
        public static void ResolveSingleBilateral(RigidBody body1, ref IndexedVector3 pos1,
                                                  RigidBody body2, ref IndexedVector3 pos2,
                                                  float distance, ref IndexedVector3 normal, ref float impulse, float timeStep)
        {
            float normalLenSqr = normal.LengthSquared();

            Debug.Assert(Math.Abs(normalLenSqr) < 1.1f);
            if (normalLenSqr > 1.1f)
            {
                impulse = 0f;
                return;
            }
            IndexedVector3 rel_pos1 = pos1 - body1.GetCenterOfMassPosition();
            IndexedVector3 rel_pos2 = pos2 - body2.GetCenterOfMassPosition();
            //this jacobian entry could be re-used for all iterations

            IndexedVector3 vel1 = body1.GetVelocityInLocalPoint(ref rel_pos1);
            IndexedVector3 vel2 = body2.GetVelocityInLocalPoint(ref rel_pos2);
            IndexedVector3 vel  = vel1 - vel2;

            IndexedBasisMatrix m1 = body1.GetCenterOfMassTransform()._basis.Transpose();
            IndexedBasisMatrix m2 = body2.GetCenterOfMassTransform()._basis.Transpose();


            JacobianEntry jac = new JacobianEntry(m1, m2, rel_pos1, rel_pos2, normal,
                                                  body1.GetInvInertiaDiagLocal(), body1.GetInvMass(),
                                                  body2.GetInvInertiaDiagLocal(), body2.GetInvMass());

            float jacDiagAB    = jac.GetDiagonal();
            float jacDiagABInv = 1f / jacDiagAB;


            float rel_vel = jac.GetRelativeVelocity(
                body1.GetLinearVelocity(),
                body1.GetCenterOfMassTransform()._basis.Transpose() * body1.GetAngularVelocity(),
                body2.GetLinearVelocity(),
                body2.GetCenterOfMassTransform()._basis.Transpose() * body2.GetAngularVelocity());
            float a = jacDiagABInv;

            rel_vel = normal.Dot(ref vel);

            //todo: move this into proper structure
            float contactDamping = 0.2f;

            if (ONLY_USE_LINEAR_MASS)
            {
                float massTerm = 1f / (body1.GetInvMass() + body2.GetInvMass());
                impulse = -contactDamping * rel_vel * massTerm;
            }
            else
            {
                float velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
                impulse = velocityImpulse;
            }
        }
Exemplo n.º 2
0
        //response  between two dynamic objects without friction, assuming 0 penetration depth
        public static float ResolveSingleCollision(
            RigidBody body1,
            CollisionObject colObj2,
            ref IndexedVector3 contactPositionWorld,
            ref IndexedVector3 contactNormalOnB,
            ContactSolverInfo solverInfo,
            float distance)
        {
            RigidBody body2 = RigidBody.Upcast(colObj2);


            IndexedVector3 normal = contactNormalOnB;

            IndexedVector3 rel_pos1 = contactPositionWorld - body1.GetWorldTransform()._origin;
            IndexedVector3 rel_pos2 = contactPositionWorld - colObj2.GetWorldTransform()._origin;

            IndexedVector3 vel1 = body1.GetVelocityInLocalPoint(ref rel_pos1);
            IndexedVector3 vel2 = body2 != null?body2.GetVelocityInLocalPoint(ref rel_pos2) : IndexedVector3.Zero;

            IndexedVector3 vel     = vel1 - vel2;
            float          rel_vel = normal.Dot(ref vel);

            float combinedRestitution = body1.GetRestitution() * colObj2.GetRestitution();
            float restitution         = combinedRestitution * -rel_vel;

            float positionalError = solverInfo.m_erp * -distance / solverInfo.m_timeStep;
            float velocityError   = -(1.0f + restitution) * rel_vel;          // * damping;
            float denom0          = body1.ComputeImpulseDenominator(ref contactPositionWorld, ref normal);
            float denom1          = body2 != null?body2.ComputeImpulseDenominator(ref contactPositionWorld, ref normal) : 0.0f;

            float relaxation   = 1.0f;
            float jacDiagABInv = relaxation / (denom0 + denom1);

            float penetrationImpulse = positionalError * jacDiagABInv;
            float velocityImpulse    = velocityError * jacDiagABInv;

            float normalImpulse = penetrationImpulse + velocityImpulse;

            normalImpulse = 0.0f > normalImpulse ? 0.0f : normalImpulse;

            body1.ApplyImpulse(normal * (normalImpulse), rel_pos1);
            if (body2 != null)
            {
                body2.ApplyImpulse(-normal * (normalImpulse), rel_pos2);
            }

            return(normalImpulse);
        }
        //
        // solve unilateral raint (equality, direct method)
        //
        public void ResolveUnilateralPairConstraint(RigidBody body0, RigidBody body1, ref IndexedBasisMatrix world2A,
                                                    ref IndexedBasisMatrix world2B,
                                                    ref IndexedVector3 invInertiaADiag,
                                                    float invMassA,
                                                    ref IndexedVector3 linvelA, ref IndexedVector3 angvelA,
                                                    ref IndexedVector3 rel_posA1,
                                                    ref IndexedVector3 invInertiaBDiag,
                                                    float invMassB,
                                                    ref IndexedVector3 linvelB, ref IndexedVector3 angvelB,
                                                    ref IndexedVector3 rel_posA2,
                                                    float depthA, ref IndexedVector3 normalA,
                                                    ref IndexedVector3 rel_posB1, ref IndexedVector3 rel_posB2,
                                                    float depthB, ref IndexedVector3 normalB,
                                                    out float imp0, out float imp1)
        {
            //(void)linvelA;
            //(void)linvelB;
            //(void)angvelB;
            //(void)angvelA;

            imp0 = 0f;
            imp1 = 0f;

            float len = Math.Abs(normalA.Length()) - 1f;

            if (Math.Abs(len) >= MathUtil.SIMD_EPSILON)
            {
                return;
            }

            Debug.Assert(len < MathUtil.SIMD_EPSILON);

            //this jacobian entry could be re-used for all iterations
            JacobianEntry jacA = new JacobianEntry(ref world2A, ref world2B, ref rel_posA1, ref rel_posA2, ref normalA, ref invInertiaADiag, invMassA,
                                                   ref invInertiaBDiag, invMassB);
            JacobianEntry jacB = new JacobianEntry(ref world2A, ref world2B, ref rel_posB1, ref rel_posB2, ref normalB, ref invInertiaADiag, invMassA,
                                                   ref invInertiaBDiag, invMassB);

            // float vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
            // float vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);

            float vel0 = IndexedVector3.Dot(normalA, (body0.GetVelocityInLocalPoint(ref rel_posA1) - body1.GetVelocityInLocalPoint(ref rel_posA1)));
            float vel1 = IndexedVector3.Dot(normalB, (body0.GetVelocityInLocalPoint(ref rel_posB1) - body1.GetVelocityInLocalPoint(ref rel_posB1)));

            //	float penetrationImpulse = (depth*contactTau*timeCorrection)  * massTerm;//jacDiagABInv
            float massTerm = 1f / (invMassA + invMassB);


            // calculate rhs (or error) terms
            float dv0 = depthA * m_tau * massTerm - vel0 * m_damping;
            float dv1 = depthB * m_tau * massTerm - vel1 * m_damping;


            // dC/dv * dv = -C

            // jacobian * impulse = -error
            //

            //impulse = jacobianInverse * -error

            // inverting 2x2 symmetric system (offdiagonal are equal!)
            //


            float nonDiag = jacA.GetNonDiagonal(jacB, invMassA, invMassB);
            float invDet  = 1f / (jacA.GetDiagonal() * jacB.GetDiagonal() - nonDiag * nonDiag);

            //imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
            //imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;

            imp0 = dv0 * jacA.GetDiagonal() * invDet + dv1 * -nonDiag * invDet;
            imp1 = dv1 * jacB.GetDiagonal() * invDet + dv0 * -nonDiag * invDet;

            //[a b]								  [d -c]
            //[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc)

            //[jA nD] * [imp0] = [dv0]
            //[nD jB]   [imp1]   [dv1]
        }