I'm stuck at creating an oscillator. Basic idea is to create a two mass oscillator linking the bodies to each other with a user joint. I figured out how theoretically could work but can't get it working. The initialization is shown below (2 bodies and the user joint):
- Code: Select all
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.));
The callback for the spring is:
- 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);
}
So can anyone tell me how to do it right?
Best regards.