Exemplo n.º 1
0
  /* (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;
    }
  }
Exemplo n.º 2
0
  /* (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);
    }
  }
Exemplo n.º 3
0
  /* (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));
  }
Exemplo n.º 4
0
  /* (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;
  }
Exemplo n.º 5
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 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));
  }
Exemplo n.º 6
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));
  }