Spring / Suspension

[pwagner]

Hey.

I'm searching for a way to implement a spring between two objects. I already tried different things but can't get it to work so maybe someone can help and give a working examample or at least a hint how to simulate the spring.
Below is some code I hoped would simulate a spring. JointData is a struct which holds stiffness, damping and resting length of the spring.

Code: Select all

void setUpExperiment() {
   float radius = 1.;
   float mass = 2.;
   dVector inertia;

   // Ball1 collision shape and body
   dMatrix offset1 = GetIdentityMatrix();
   offset1.m_posit = dVector(0., 0., 0., 1.);

   dMatrix origin1 = GetIdentityMatrix();
   origin1.m_posit = dVector(0., 2., 0., 1.);
   
   ball1Collision = NewtonCreateSphere(world, radius, radius, radius, --shapeId, &offset1[0][0]);
   ball1Body = NewtonCreateBody(world, ball1Collision, &origin1[0][0]);

   NewtonBodySetLinearDamping(ball1Body, 0.f);
   NewtonConvexCollisionCalculateInertialMatrix(ball1Collision, &inertia[0], &origin1[0][0]);
   NewtonBodySetMassMatrix(ball1Body, mass, mass * inertia.m_x, mass * inertia.m_y, mass * inertia.m_z);
   NewtonBodySetCentreOfMass(ball1Body, &origin1[0][0]);
   NewtonBodySetMaterialGroupID(ball1Body, materials["default_material"]);
   NewtonBodySetForceAndTorqueCallback (ball1Body, ForceAndTorqueCallback);
   
   NewtonReleaseCollision(world, ball1Collision);

   sensors.push_back(ball1Body);

   // Ball2 collision shape and body
   dMatrix offset2 = GetIdentityMatrix();
   offset2.m_posit = dVector(0., 0., 0., 1.);

   dMatrix origin2 = GetIdentityMatrix();
   origin2.m_posit = dVector(0., 0., 0., 1.);

   ball2Collision = NewtonCreateSphere(world, radius, radius, radius, --shapeId, &offset2[0][0]);
   ball2Body = NewtonCreateBody(world, ball2Collision, &origin2[0][0]);

   NewtonBodySetLinearDamping(ball2Body, 0.f);
   NewtonConvexCollisionCalculateInertialMatrix(ball2Collision, &inertia[0], &origin2[0][0]);
   NewtonBodySetMassMatrix(ball2Body, mass, mass * inertia.m_x, mass * inertia.m_y, mass * inertia.m_z);
   NewtonBodySetCentreOfMass(ball2Body, &origin2[0][0]);
   NewtonBodySetMaterialGroupID(ball2Body, materials["default_material"]);
   NewtonBodySetForceAndTorqueCallback (ball2Body, ForceAndTorqueCallback);
   
   NewtonReleaseCollision(world, ball2Collision);

   spring = NewtonConstraintCreateUserJoint(world, 1, SpringCallback, NULL, ball2Body, ball1Body);
   NewtonJointSetUserData(spring, (void*) &JointData(500, 0.5, 3.));

   initialConditions();
}


Code: Select all

static void SpringCallback(const NewtonJoint* joint, float timestep, int threadIndex) {

   JointData* data = (JointData*) NewtonJointGetUserData(joint);

   dMatrix matrix = GetIdentityMatrix();
   
   const NewtonBody* body1 = NewtonJointGetBody0(joint);
   const NewtonBody* body2 = NewtonJointGetBody1(joint);
   NewtonBodyGetMatrix(body1, &matrix[0][0]);
   dVector position1 = matrix.m_posit;

   NewtonBodyGetMatrix(body2, &matrix[0][0]);
   dVector position2 = matrix.m_posit;

   dVector axis = position1 - position2;
   axis = axis.Scale(1./(axis % axis));

   NewtonUserJointAddLinearRow(joint, &position1[0], &position2[0], &axis[0]);
   NewtonUserJointSetRowSpringDamperAcceleration(joint, data->stiffness, data->damping);

//   dFloat acceleration = NewtonCalculateSpringDamperAcceleration(timestep, data->stiffness, data->restLength, data->damping, axis % axis);
   
   /*NewtonBody* body1 = NewtonJointGetBody0(joint);
   NewtonBody* body2 = NewtonJointGetBody1(joint);

   JointData* data = (JointData*) NewtonJointGetUserData(joint);
   
   dMatrix matrix = GetIdentityMatrix();
   
   NewtonBodyGetMatrix(body1, &matrix[0][0]);
   dVector position1 = matrix.m_posit;

   NewtonBodyGetMatrix(body2, &matrix[0][0]);
   dVector position2 = matrix.m_posit;

   dVector axis = position1 - position2;
   double distance = sqrt(axis % axis);
   double elongation = distance - data->restLength;
   axis = axis.Scale(1./(axis % axis));

   dVector velocity1 = dVector();
   NewtonBodyGetVelocity(body1, &velocity1[0]);

   dVector velocity2 = dVector();
   NewtonBodyGetVelocity(body2, &velocity2[0]);

   double relativeVelocity = sqrt((velocity1 - velocity2) % (velocity1 - velocity2));

   double force = -data->stiffness * elongation/2.;
   
   dVector tmp = position1 + axis.Scale(distance) - position2;
   if(tmp % tmp == 0) {
      axis = axis.Scale(force);
   } else {
      axis = axis.Scale(-force);
   }

   NewtonBodyAddForce(body1, &axis[0]);
   axis = axis.Scale(-1);
   NewtonBodyAddForce(body2, &axis[0]);*/
}


[pwagner]

Although nobody seems to know newton well enough to give an answer I want to post my progress to those who have similar problems. I managed to get a spring like behavior although its still not working the way I intended it to be. The UserJoint is now replaced by a slider with a similar callback function posted above.

Code: Select all

static unsigned int SpringCallback(const NewtonJoint* joint, NewtonHingeSliderUpdateDesc* tmp) {

   JointData* data = (JointData*) NewtonJointGetUserData(joint);

   dMatrix matrix = GetIdentityMatrix();
   
   const NewtonBody* body1 = NewtonJointGetBody0(joint);
   const NewtonBody* body2 = NewtonJointGetBody1(joint);
   
   NewtonBodyGetMatrix(body1, &matrix[0][0]);
   dVector position1 = matrix.m_posit;

   NewtonBodyGetMatrix(body2, &matrix[0][0]);
   dVector position2 = matrix.m_posit;

   dVector axis = position1 - position2;
   dFloat distance = sqrt(axis % axis) - data->restLength;
//   dFloat distance = data->restLength - sqrt(axis % axis);
   axis = axis.Scale(1./(distance));

   dVector speed1(0.,0., 0., 0.);
   NewtonBodyGetVelocity(body1, &speed1[0]);

   dVector speed2(0.,0., 0., 0.);
   NewtonBodyGetVelocity(body1, &speed2[0]);

   dVector speedVector = speed1 - speed2;
   dFloat speed = sqrt(speedVector % speedVector);
//   dFloat speed = NewtonSliderGetJointVeloc(joint);

   //NewtonUserJointAddLinearRow(joint, &position1[0], &position2[0], &matrix.m_up[0]);

   dFloat acceleration = -data->stiffness * distance - data->damping * speed;
   dVector force(0., 0., 0., 0.);
   if(speed < 0.) {
      force = axis.Scale(1./speed * acceleration);
   } else {
      force = axis.Scale(725.*acceleration);
   }

   NewtonBodyAddForce(body1, &force[0]);
   force = force.Scale(-1);
   NewtonBodyAddForce(body2, &force[0]);
   
   return 1;
}


The most annoying part is the fact that both bodies return a velocity of 0 and they are interpenetrating each other. As for the scaling factor of 725 I think it should be replaced by something like the (inverse?) effective mass of the connected bodies. I will keep working on it and maybe in the meantime someone can help.