Simple springJoint

Posts that don't fit into other categories.
maheshkurmi
Posts: 63
Joined: Tue Mar 27, 2012 7:20 am
Location: India
Contact:

Simple springJoint

Postby maheshkurmi » Fri Mar 22, 2013 5:52 am

I have created a new spring joint which can behave as an ideal spring (zero damping) , It is working same as distance joint as spring damper and now we can have zero damping in it, since there is no soft constraint involved.
I have used impulse equations in initializeConstraints() function, and kept solveVelocityConstraints() empty. Do I need to update impulse equation in
solveVelocityConstraints() for better accuracy?

Code: Select all

package org.dyn4j.dynamics.joint;

import org.dyn4j.Epsilon;
import org.dyn4j.dynamics.Body;
import org.dyn4j.dynamics.Settings;
import org.dyn4j.dynamics.Step;
import org.dyn4j.geometry.Geometry;
import org.dyn4j.geometry.Mass;
import org.dyn4j.geometry.Transform;
import org.dyn4j.geometry.Vector2;
import org.dyn4j.resources.Messages;

public class SpringJoint extends Joint {
   /** The local anchor point on the first {@link Body} */
   protected Vector2 localAnchor1;
   
   /** The local anchor point on the second {@link Body} */
   protected Vector2 localAnchor2;
   
   /**Spring constant in N/m2*/
   private double springConstant =500;
   
   /** The damping ratio */
   protected double dampingCoeff;
   
   /** The Natural Length of spring (computed distance between the two world space anchor points) */
   protected double naturalLength;
   
   /** The normal */
   protected Vector2 n;
   
   /** The accumulated impulse from the previous time step */
   protected double impulse;
   
   /** The effective mass of the two body system (Reduced mass translational+Rotational) */
   protected double invEqMass;
   
   /** The oscillation frequency in hz */
   protected double frequency;

   /** if the Spring is harmonic oscillator (keeps frequency constant)*/
   protected boolean oscillator=false;
   
   /**
    * Minimal constructor.
    * <p>
    * Creates a fixed distance {@link Joint} where the joined
    * {@link Body}s do not participate in collision detection and
    * resolution.
    * @param body1 the first {@link Body}
    * @param body2 the second {@link Body}
    * @param anchor1 in world coordinates
    * @param anchor2 in world coordinates
    * @param k spring constant
    * @param damping Coefficient
    * @throws NullPointerException if body1, body2, anchor1, or anchor2 is null
    * @throws IllegalArgumentException if body1 == body2, k<=0, dampingCoeff<0;
    */
   public SpringJoint(Body body1, Body body2, Vector2 anchor1, Vector2 anchor2, double k,double dampingCoeff) {
      super(body1, body2, false);
      // verify the bodies are not the same instance
      if (body1 == body2) throw new IllegalArgumentException(Messages.getString("dynamics.joint.sameBody"));
      // verify the anchor points are not null
      if (anchor1 == null) throw new NullPointerException(Messages.getString("dynamics.joint.nullAnchor1"));
      if (anchor2 == null) throw new NullPointerException(Messages.getString("dynamics.joint.nullAnchor2"));
      // get the local anchor points
      this.localAnchor1 = body1.getLocalPoint(anchor1);
      this.localAnchor2 = body2.getLocalPoint(anchor2);
      if(k<=0)throw new IllegalArgumentException("ForceConstant can not be less than or equal to zero");
      if(dampingCoeff<0)throw new IllegalArgumentException("DampingCoeff can not be less than zero");
      this.springConstant=k;
      this.dampingCoeff=dampingCoeff;
      // compute the initial distance
      this.naturalLength = anchor1.distance(anchor2);
   }
   
   /* (non-Javadoc)
    * @see org.dyn4j.dynamics.joint.Joint#initializeConstraints()
    */
   @Override
   public void initializeConstraints() {
      Step step = this.world.getStep();
      Settings settings = this.world.getSettings();
      
      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()));
      
      // compute effective mass
      double cr1n = r1.cross(this.n);
      double cr2n = r2.cross(this.n);
      double invMass = invM1 + invI1 * cr1n * cr1n;
      invMass += invM2 + invI2 * cr2n * cr2n;
      
      //Temporary value of force constant k
      double k;
      if (oscillator){
         // compute the natural frequency; f = w / (2 * pi) -> w = 2 * pi * f
         double w = Geometry.TWO_PI * this.frequency;
         // check for zero before inverting
         double massEff= invMass <= Epsilon.E ? 0.0 : 1.0 / invMass;
         k = massEff * w * w;
      }else{
         k=this.springConstant;
         this.frequency= Math.sqrt(this.springConstant*this.invEqMass)/(2*Math.PI);
      }
         
      // 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);
      }
      double dt = step.getDeltaTime();
      // get the current compression/extension of the spring
      double x = length - this.naturalLength;
      
      // compute the speeds of anchor points of spring (v=v0+rw)
      Vector2 v1 = body1.getVelocity().sum(r1.cross(body1.getAngularVelocity()));
      Vector2 v2 = body2.getVelocity().sum(r2.cross(body2.getAngularVelocity()));
            
      // compute relative speed of anchor points of spring(rate of change of length of spring)
      double rv = n.dot(v1.difference(v2));
      
      //use force =-kx-bv
      this.impulse=-k*x*dt-dampingCoeff*rv*dt;
            
      Vector2 J = n.product(impulse);
      body1.getVelocity().add(J.product(invM1));
      body1.setAngularVelocity(body1.getAngularVelocity() + invI1 * r1.cross(J));
      body2.getVelocity().subtract(J.product(invM2));
      body2.setAngularVelocity(body2.getAngularVelocity() - invI2 * r2.cross(J));
   }
   
   /* (non-Javadoc)
    * @see org.dyn4j.dynamics.joint.Joint#solveVelocityConstraints()
    */
   @Override
   public void solveVelocityConstraints() {

   }
   
   @Override
   public boolean solvePositionConstraints() {
      // don't solve position constraints for spring damper
      return true;
   }

   /* (non-Javadoc)
    * @see org.dyn4j.dynamics.joint.Joint#getReactionForce(double)
    */
   @Override
   public Vector2 getReactionForce(double invdt) {
      return this.n.product(this.impulse * invdt);
   }
   
   /* (non-Javadoc)
    * @see org.dyn4j.dynamics.joint.Joint#getReactionTorque(double)
    */
   @Override
   public double getReactionTorque(double invdt) {
      return 0.0;
   }
   
   /**
    * Returns true if this distance joint is a spring distance joint
    * with damping.
    * @return boolean
    */
   public boolean isSpringDamper() {
      return this.dampingCoeff > 0.0;
   }
   
   /**
    * Returns the damping ratio.
    * @return double
    */
   public double getDampingcoeff() {
      return this.dampingCoeff;
   }
   
   /**
    * Sets the damping ratio.
    * @param dampingRatio the damping ratio; in the range [0, 1]
    * @throws IllegalArgumentException if damping ration is less than zero or greater than 1
    */
   public void setDampingCoeff(double dampingCoeff) {
      // make sure its within range
      if (dampingCoeff < 0 || dampingCoeff > 1) throw new IllegalArgumentException("DampingCoeff can not be less than or equal to zero");
      // set the new value
      this.dampingCoeff = dampingCoeff;
   }
   
   /**
    * Returns the spring frequency.
    * @return double
    */
   public double getFrequency() {
      if (oscillator){
         return this.frequency;
      }else{
         return Math.sqrt(this.springConstant*this.invEqMass)/(2*Math.PI);
      }
   }
   
   /**
    * Sets the spring frequency.
    * @param frequency the spring frequency in hz; must be greater than or equal to zero
    * @throws IllegalArgumentException if frequency is less than zero
    */
   public void setFrequency(double frequency) {
      // check for valid value
      if (frequency < 0) throw new IllegalArgumentException("frequency can not be less than or equal to zero");
      // set the new value
      this.frequency = frequency;
   }

   /**
    * Returns the rest distance between the two constrained {@link Body}s in meters.
    * @return double
    */
   public double getNaturalLength() {
      return this.naturalLength;
   }
   
   /**
    * Sets the rest distance between the two constrained {@link Body}s in meters.
    * @param naturalLength the naturalLength in meters
    * @throws IllegalArgumentException if distance is less than zero
    */
   public void setNaturalLength(double naturalLength) {
      // make sure the distance is greater than zero
      if (naturalLength < 0.0) throw new IllegalArgumentException("Natural Length can not be less than zero");
      // wake up both bodies
      this.body1.setAsleep(false);
      this.body2.setAsleep(false);
      // set the new target distance
      this.naturalLength = naturalLength;
   }

   /**
    * Sets Spring Constant of the spring
    * @param springConstant
    */
   public void setSpringConstant(double springConstant){
      // make sure springConstant is greater than zero
      if (springConstant < 0.0) throw new IllegalArgumentException("ForceConstant can not be less than or equal to zero");
      this.springConstant=springConstant;
   }

   /**
    * returns Spring Constant of the Spring
    * @return Spring Constant of the Spring
    */
   public double getSpringConstant(){
      return this.springConstant;
   }
   
   /* (non-Javadoc)
    * @see org.dyn4j.dynamics.joint.Joint#getAnchor1()
    */
   public Vector2 getAnchor1() {
      return body1.getWorldPoint(this.localAnchor1);
   }
   
   /* (non-Javadoc)
    * @see org.dyn4j.dynamics.joint.Joint#getAnchor2()
    */
   public Vector2 getAnchor2() {
      return body2.getWorldPoint(this.localAnchor2);
   }

   @Override
   protected void shiftCoordinates(Vector2 shift) {
      // nothing to translate here since the anchor points are in local coordinates
      // they will move with the bodies
   }
}

William
Site Admin
Posts: 378
Joined: Sat Feb 06, 2010 10:23 pm

Re: Simple springJoint

Postby William » Fri Mar 22, 2013 2:05 pm

The initializeConstraints method is called once. Afterwards the solveVelocityConstraints method is called n times, then the solvePositionConstrants method is called m times.

You could probably take the DistanceJoint class, remove gamma and bias from the calculations and get an ideal spring and leave pretty much everything else as is (no guarantees on that though). However, I wouldn't worry too much about this if you have something that is working for you.

William

maheshkurmi
Posts: 63
Joined: Tue Mar 27, 2012 7:20 am
Location: India
Contact:

Re: Simple springJoint

Postby maheshkurmi » Fri Mar 22, 2013 2:35 pm

Thanks for your reply, I will soon share progress of physics simulator...


Return to “General Discussion”

Who is online

Users browsing this forum: No registered users and 2 guests