/* (non-Javadoc) * @see org.dyn4j.dynamics.joint.Joint#solveVelocityConstraints(org.dyn4j.dynamics.Step, org.dyn4j.dynamics.Settings) */ @Override public void solveVelocityConstraints(Step step, Settings settings) { Transform t1 = body1.getTransform(); Transform t2 = body2.getTransform(); Mass m1 = body1.getMass(); Mass m2 = body2.getMass(); double invM1 = m1.getInverseMass(); double invM2 = m2.getInverseMass(); double invI1 = m1.getInverseInertia(); double invI2 = m2.getInverseInertia(); // compute r1 and r2 Vector2 r1 = t1.getTransformedR(this.body1.getLocalCenter().to(this.localAnchor1)); Vector2 r2 = t2.getTransformedR(this.body2.getLocalCenter().to(this.localAnchor2)); // compute the relative velocity Vector2 v1 = body1.getLinearVelocity().sum(r1.cross(body1.getAngularVelocity())); Vector2 v2 = body2.getLinearVelocity().sum(r2.cross(body2.getAngularVelocity())); // compute Jv double Jv = n.dot(v1.difference(v2)); // compute lambda (the magnitude of the impulse) double j = -this.invK * (Jv + this.bias + this.gamma * this.impulse); this.impulse += j; // apply the impulse Vector2 J = n.product(j); body1.getLinearVelocity().add(J.product(invM1)); body1.setAngularVelocity(body1.getAngularVelocity() + invI1 * r1.cross(J)); body2.getLinearVelocity().subtract(J.product(invM2)); body2.setAngularVelocity(body2.getAngularVelocity() - invI2 * r2.cross(J)); }
/* (non-Javadoc) * @see org.dyn4j.dynamics.joint.Joint#solvePositionConstraints(org.dyn4j.dynamics.Step, org.dyn4j.dynamics.Settings) */ @Override public boolean solvePositionConstraints(Step step, Settings settings) { // check if this is a spring damper if (this.frequency > 0.0) { // don't solve position constraints for spring damper return true; } double linearTolerance = settings.getLinearTolerance(); double maxLinearCorrection = settings.getMaximumLinearCorrection(); Transform t1 = body1.getTransform(); Transform t2 = body2.getTransform(); Mass m1 = body1.getMass(); Mass m2 = body2.getMass(); double invM1 = m1.getInverseMass(); double invM2 = m2.getInverseMass(); double invI1 = m1.getInverseInertia(); double invI2 = m2.getInverseInertia(); Vector2 c1 = body1.getWorldCenter(); Vector2 c2 = body2.getWorldCenter(); // recompute n since it may have changed after integration Vector2 r1 = t1.getTransformedR(this.body1.getLocalCenter().to(this.localAnchor1)); Vector2 r2 = t2.getTransformedR(this.body2.getLocalCenter().to(this.localAnchor2)); n = r1.sum(body1.getWorldCenter()).subtract(r2.sum(body2.getWorldCenter())); // solve the position constraint double l = n.normalize(); double C = l - this.distance; C = Interval.clamp(C, -maxLinearCorrection, maxLinearCorrection); double impulse = -this.invK * C; Vector2 J = n.product(impulse); // translate and rotate the objects body1.translate(J.product(invM1)); body1.rotate(invI1 * r1.cross(J), c1); body2.translate(J.product(-invM2)); body2.rotate(-invI2 * r2.cross(J), c2); return Math.abs(C) < linearTolerance; }
/* (non-Javadoc) * @see org.dyn4j.dynamics.joint.Joint#initializeConstraints(org.dyn4j.dynamics.Step, org.dyn4j.dynamics.Settings) */ @Override public void initializeConstraints(Step step, Settings settings) { double linearTolerance = settings.getLinearTolerance(); Transform t1 = body1.getTransform(); Transform t2 = body2.getTransform(); Mass m1 = body1.getMass(); Mass m2 = body2.getMass(); double invM1 = m1.getInverseMass(); double invM2 = m2.getInverseMass(); double invI1 = m1.getInverseInertia(); double invI2 = m2.getInverseInertia(); // compute the normal Vector2 r1 = t1.getTransformedR(this.body1.getLocalCenter().to(this.localAnchor1)); Vector2 r2 = t2.getTransformedR(this.body2.getLocalCenter().to(this.localAnchor2)); this.n = r1.sum(this.body1.getWorldCenter()).subtract(r2.sum(this.body2.getWorldCenter())); // get the current length double length = this.n.getMagnitude(); // check for the tolerance if (length < linearTolerance) { this.n.zero(); } else { // normalize it this.n.multiply(1.0 / length); } // compute K inverse double cr1n = r1.cross(this.n); double cr2n = r2.cross(this.n); double invMass = invM1 + invI1 * cr1n * cr1n; invMass += invM2 + invI2 * cr2n * cr2n; // check for zero before inverting this.invK = invMass <= Epsilon.E ? 0.0 : 1.0 / invMass; // see if we need to compute spring damping if (this.frequency > 0.0) { double dt = step.getDeltaTime(); // get the current compression/extension of the spring double x = length - this.distance; // compute the natural frequency; f = w / (2 * pi) -> w = 2 * pi * f double w = Geometry.TWO_PI * this.frequency; // compute the damping coefficient; dRatio = d / (2 * m * w) -> d = 2 * m * w * dRatio double d = 2.0 * this.invK * this.dampingRatio * w; // compute the spring constant; w = sqrt(k / m) -> k = m * w * w double k = this.invK * w * w; // compute gamma = CMF = 1 / (hk + d) this.gamma = dt * (d + dt * k); // check for zero before inverting this.gamma = this.gamma <= Epsilon.E ? 0.0 : 1.0 / this.gamma; // compute the bias = x * ERP where ERP = hk / (hk + d) this.bias = x * dt * k * this.gamma; // compute the effective mass invMass += this.gamma; // check for zero before inverting this.invK = invMass <= Epsilon.E ? 0.0 : 1.0 / invMass; } else { this.gamma = 0.0; this.bias = 0.0; } // warm start impulse *= step.getDeltaTimeRatio(); Vector2 J = n.product(impulse); body1.getLinearVelocity().add(J.product(invM1)); body1.setAngularVelocity(body1.getAngularVelocity() + invI1 * r1.cross(J)); body2.getLinearVelocity().subtract(J.product(invM2)); body2.setAngularVelocity(body2.getAngularVelocity() - invI2 * r2.cross(J)); }