/* (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 the constraint needs to be applied if (this.limitState != LimitState.INACTIVE) { double angularTolerance = settings.getAngularTolerance(); double maxAngularCorrection = settings.getMaximumAngularCorrection(); Mass m1 = this.body1.getMass(); Mass m2 = this.body2.getMass(); double invI1 = m1.getInverseInertia(); double invI2 = m2.getInverseInertia(); // get the current angle between the bodies double angle = this.getRelativeRotation(); double impulse = 0.0; double angularError = 0.0; // check the limit state if (this.limitState == LimitState.EQUAL) { // if the limits are equal then clamp the impulse to maintain // the constraint between the maximum double j = Interval.clamp(angle - this.lowerLimit, -maxAngularCorrection, maxAngularCorrection); impulse = -j * this.invK; angularError = Math.abs(j); } else if (this.limitState == LimitState.AT_LOWER) { // if the joint is at the lower limit then clamp only the lower value double j = angle - this.lowerLimit; angularError = -j; j = Interval.clamp(j + angularTolerance, -maxAngularCorrection, 0.0); impulse = -j * this.invK; } else if (this.limitState == LimitState.AT_UPPER) { // if the joint is at the upper limit then clamp only the upper value double j = angle - this.upperLimit; angularError = j; j = Interval.clamp(j - angularTolerance, 0.0, maxAngularCorrection); impulse = -j * this.invK; } // apply the corrective impulses to the bodies this.body1.rotateAboutCenter(invI1 * impulse); this.body2.rotateAboutCenter(-invI2 * impulse); return angularError <= angularTolerance; } else { return true; } }
/* (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) { Mass m1 = this.body1.getMass(); Mass m2 = this.body2.getMass(); double invI1 = m1.getInverseInertia(); double invI2 = m2.getInverseInertia(); // check if the limit needs to be applied (if we are at one of the limits // then we ignore the ratio) if (this.limitState != LimitState.INACTIVE) { // solve the angular constraint // get the relative velocity double C = this.body1.getAngularVelocity() - this.body2.getAngularVelocity(); // get the impulse required to obtain the speed double impulse = this.invK * -C; if (this.limitState == LimitState.EQUAL) { this.impulse += impulse; } else if (this.limitState == LimitState.AT_LOWER) { double newImpulse = this.impulse + impulse; if (newImpulse < 0.0) { impulse = -this.impulse; this.impulse = 0.0; } } else if (this.limitState == LimitState.AT_UPPER) { double newImpulse = this.impulse + impulse; if (newImpulse > 0.0) { impulse = -this.impulse; this.impulse = 0.0; } } // apply the impulse this.body1.setAngularVelocity(this.body1.getAngularVelocity() + invI1 * impulse); this.body2.setAngularVelocity(this.body2.getAngularVelocity() - invI2 * impulse); } else if (this.ratio != 1.0) { // the limit is inactive and the ratio is not one // get the relative velocity double C = this.body1.getAngularVelocity() - this.ratio * this.body2.getAngularVelocity(); // get the impulse required to obtain the speed double impulse = this.invK * -C; // apply the impulse this.body1.setAngularVelocity(this.body1.getAngularVelocity() + invI1 * impulse); this.body2.setAngularVelocity(this.body2.getAngularVelocity() - invI2 * impulse * this.ratio); } }
/* (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 angularTolerance = settings.getAngularTolerance(); Mass m1 = this.body1.getMass(); Mass m2 = this.body2.getMass(); double invI1 = m1.getInverseInertia(); double invI2 = m2.getInverseInertia(); // check if the limits are enabled if (this.limitEnabled) { // compute the current angle double angle = this.getRelativeRotation(); // if they are enabled check if they are equal if (Math.abs(this.upperLimit - this.lowerLimit) < 2.0 * angularTolerance) { // if so then set the state to equal this.limitState = LimitState.EQUAL; } else { // make sure we have valid settings if (this.upperLimit > this.lowerLimit) { // check against the max and min distances if (angle >= this.upperLimit) { // is the limit already at the upper limit if (this.limitState != LimitState.AT_UPPER) { this.impulse = 0; } // set the state to at upper this.limitState = LimitState.AT_UPPER; } else if (angle <= this.lowerLimit) { // is the limit already at the lower limit if (this.limitState != LimitState.AT_LOWER) { this.impulse = 0; } // set the state to at lower this.limitState = LimitState.AT_LOWER; } else { // set the state to inactive this.limitState = LimitState.INACTIVE; this.impulse = 0; } } } } else { // neither is enabled so no constraint needed at this time this.limitState = LimitState.INACTIVE; this.impulse = 0; } // compute the mass if (this.limitState == LimitState.INACTIVE) { // compute the angular mass including the ratio this.invK = invI1 + this.ratio * this.ratio * invI2; } else { // compute the angular mass normally this.invK = invI1 + invI2; } if (this.invK > Epsilon.E) { this.invK = 1.0 / this.invK; } // account for variable time step this.impulse *= step.getDeltaTimeRatio(); // warm start this.body1.setAngularVelocity(this.body1.getAngularVelocity() + invI1 * this.impulse); // we only want to apply the ratio to the impulse if the limits are not active. When the // limits are active we effectively disable the ratio this.body2.setAngularVelocity( this.body2.getAngularVelocity() - invI2 * this.impulse * (this.limitState == LimitState.INACTIVE ? this.ratio : 1.0)); }
/* (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)); }