dReal ServoMotor::getTorque() {
// code from Jeff Shim
osg::Vec3 torque1(fback_.t1[0], fback_.t1[1], fback_.t1[2] );
osg::Vec3 torque2(fback_.t2[0], fback_.t2[1], fback_.t2[2] );
osg::Vec3 force1(fback_.f1[0], fback_.f1[1], fback_.f1[2] );
osg::Vec3 force2(fback_.f2[0], fback_.f2[1], fback_.f2[2] );
const double* p1 = dBodyGetPosition( dJointGetBody(joint_->getJoint(),0) );
const double* p2 = dBodyGetPosition( dJointGetBody(joint_->getJoint(),1) );
osg::Vec3 pos1(p1[0], p1[1], p1[2]);
osg::Vec3 pos2(p2[0], p2[1], p2[2]);
dVector3 odeAnchor;
dJointGetHingeAnchor ( joint_->getJoint(), odeAnchor );
osg::Vec3 anchor(odeAnchor[0], odeAnchor[1], odeAnchor[2]);
osg::Vec3 ftorque1 = torque1 - (force1^(pos1-anchor));// torq by motor = total torq - constraint torq
osg::Vec3 ftorque2 = torque2 - (force2^(pos2-anchor));// opposite direction - use if this is necessary
dVector3 odeAxis;
dJointGetHingeAxis ( joint_->getJoint(), odeAxis);
osg::Vec3 axis(odeAxis[0], odeAxis[1], odeAxis[2] );
axis.normalize();
double torque = ftorque1 * axis;
//printf ("torque: % 1.10f\n", torque);
return torque;
}
/**
* @brief Moves particles forward based on 2nd order Runge-Kutta method
*
* @param[in] endTime Final time of the simulation
**/
void Runge_Kutta2::moveParticles(const double endTime) {
while (currentTime < endTime) {
const double dt = modifyTimeStep(1.0e-4f, init_dt); // implement dynamic timstep (if necessary):
const doubleV vdt = set1_pd(dt); // store timestep as vector const
const cloud_index numParticles = cloud->n;
operate1(currentTime);
force1(currentTime); // compute net force1
BEGIN_PARALLEL_FOR(i, e, numParticles, DOUBLE_STRIDE, static) // calculate k1 and l1 for entire cloud
const doubleV vmass = load_pd(cloud->mass + i); // load ith and (i+1)th mass into vector
// assign force pointers for stylistic purposes:
double * const pFx = cloud->forceX + i;
double * const pFy = cloud->forceY + i;
store_pd(cloud->k1 + i, div_pd(mul_pd(vdt, load_pd(pFx)), vmass)); // velocityX tidbit
store_pd(cloud->l1 + i, mul_pd(vdt, cloud->getVx1_pd(i))); // positionX tidbit
store_pd(cloud->m1 + i, div_pd(mul_pd(vdt, load_pd(pFy)), vmass)); // velocityY tidbit
store_pd(cloud->n1 + i, mul_pd(vdt, cloud->getVy1_pd(i))); // positionY tidbit
// reset forces to zero:
store_pd(pFx, set0_pd());
store_pd(pFy, set0_pd());
END_PARALLEL_FOR
operate2(currentTime + dt/2.0);
force2(currentTime + dt/2.0); // compute net force2
BEGIN_PARALLEL_FOR(i, e, numParticles, DOUBLE_STRIDE, static) // calculate k2 and l for entire cloud
const doubleV vmass = load_pd(cloud->mass + i);
// assign force pointers:
double * const pFx = cloud->forceX + i;
double * const pFy = cloud->forceY + i;
store_pd(cloud->k2 + i, div_pd(mul_pd(vdt, load_pd(pFx)), vmass)); // velocityX tidbit
store_pd(cloud->l2 + i, mul_pd(vdt, cloud->getVx2_pd(i))); // positionX tidbit
store_pd(cloud->m2 + i, div_pd(mul_pd(vdt, load_pd(pFy)), vmass)); // velocityY tidbit
store_pd(cloud->n2 + i, mul_pd(vdt, cloud->getVy2_pd(i))); // positionY tidbit
// reset forces to zero:
store_pd(pFx, set0_pd());
store_pd(pFy, set0_pd());
END_PARALLEL_FOR
// Calculate next position and next velocity for entire cloud.
BEGIN_PARALLEL_FOR(i, e, numParticles, DOUBLE_STRIDE, static)
plusEqual_pd(cloud->Vx + i, load_pd(cloud->k2 + i));
plusEqual_pd(cloud->x + i, load_pd(cloud->l2 + i));
plusEqual_pd(cloud->Vy + i, load_pd(cloud->m2 + i));
plusEqual_pd(cloud->y + i, load_pd(cloud->n2 + i));
END_PARALLEL_FOR
currentTime += dt;
}
}
main()
{
double dt, dtcoef;
double tmax;
double dtout;
double time, tout;
VECT x, v;
VECT newx, newv;
scanf("%le%le%le", &dtcoef, &tmax, &dtout);
scanf("%le%le%le%le", &(x.x), &(x.y), &(v.x), &(v.y));
printf("dtcoef, tmax, dtout = %e %e %e\n", dtcoef, tmax, dtout);
printf("x, v = %e %e %e %e\n", (x.x), (x.y), (v.x), (v.y));
time=0;
tout = dtout;
printenergy(x, v, time);
while (time < tmax){
double newdt, dt0;
int i;
dt = timescale(x, v)*dtcoef;
dt0=dt;
#ifdef SIMPLE_SYMMETRIC
for (i=0;i<5; i++){
leapfrog(x,v, &newx, &newv, dt);
newdt=timescale(newx, newv)*dtcoef;
dt = 0.5*(dt0+newdt);
}
#endif
#ifdef CORRECTED_BLOCKSTEP
dt=force2(dt0);
if (fmod(time, dt*2)== 0.0) dt*=2;
for (i=0;i<5; i++){
leapfrog(x,v, &newx, &newv, dt);
newdt=timescale(newx, newv)*dtcoef;
if (dt > 0.5*(dt0+newdt)) dt *= 0.5;
}
#endif
#ifdef SIMPLE_BLOCKSTEP
#define NSYM 5
dt = force2(dt);
for (i=0;i<NSYM; i++){
leapfrog(x,v, &newx, &newv, dt);
newdt=timescale(newx, newv)*dtcoef;
if (i < NSYM-1){
dt = force2(0.5*(dt0+newdt));
}
}
#endif
#ifdef MIN_BLOCKSTEP
#define NSYM 5
{
double steps[NSYM];
dt = force2(dt);
for (i=0;i<NSYM; i++){
leapfrog(x,v, &newx, &newv, dt);
newdt=timescale(newx, newv)*dtcoef;
if (i < NSYM-1){
steps[i] = force2(0.5*(dt0+newdt));
if (i < NSYM-2) {
dt = steps[i];
}else{
dt = (steps[i]>steps[i-1])? steps[i-1]:steps[i];
}
}
}
}
#endif
time += dt;
x=newx;
v=newv;
if (time >= tout){
printv(x, "x");
printv(v, "v");
printenergy(x, v, time);
tout += dtout;
}
}
}
