real calc_cm(FILE *log,int natoms,real mass[],rvec x[],rvec v[],
rvec xcm,rvec vcm,rvec acm,matrix L)
{
rvec dx,a0;
real tm,m0;
int i,m;
clear_rvec(xcm);
clear_rvec(vcm);
clear_rvec(acm);
tm=0.0;
for(i=0; (i<natoms); i++) {
m0=mass[i];
tm+=m0;
cprod(x[i],v[i],a0);
for(m=0; (m<DIM); m++) {
xcm[m]+=m0*x[i][m]; /* c.o.m. position */
vcm[m]+=m0*v[i][m]; /* c.o.m. velocity */
acm[m]+=m0*a0[m]; /* rotational velocity around c.o.m. */
}
}
cprod(xcm,vcm,a0);
for(m=0; (m<DIM); m++) {
xcm[m]/=tm;
vcm[m]/=tm;
acm[m]-=a0[m]/tm;
}
#define PVEC(str,v) fprintf(log,\
"%s[X]: %10.5e %s[Y]: %10.5e %s[Z]: %10.5e\n", \
str,v[0],str,v[1],str,v[2])
#ifdef DEBUG
PVEC("xcm",xcm);
PVEC("acm",acm);
PVEC("vcm",vcm);
#endif
clear_mat(L);
for(i=0; (i<natoms); i++) {
m0=mass[i];
for(m=0; (m<DIM); m++)
dx[m]=x[i][m]-xcm[m];
L[XX][XX]+=dx[XX]*dx[XX]*m0;
L[XX][YY]+=dx[XX]*dx[YY]*m0;
L[XX][ZZ]+=dx[XX]*dx[ZZ]*m0;
L[YY][YY]+=dx[YY]*dx[YY]*m0;
L[YY][ZZ]+=dx[YY]*dx[ZZ]*m0;
L[ZZ][ZZ]+=dx[ZZ]*dx[ZZ]*m0;
}
#ifdef DEBUG
PVEC("L-x",L[XX]);
PVEC("L-y",L[YY]);
PVEC("L-z",L[ZZ]);
#endif
return tm;
}
int main ()
{
/* Empty input. */
CHECK (-1, (*(char *)v.i == FILL), "", "%hhd", v.i);
CHECK (-1, (*(char *)v.i == FILL), "", " %hhd", v.i);
CHECK (-1, (*(char *)v.i == FILL), " ", " %hhd", v.i);
CHECK (-1, (*(char *)v.i == FILL), " ", " %hhd", v.i);
CHECK (-1, (*(char *)v.i == FILL), "\t\n\v\f\r", " %hhd", v.i);
/* Normal conversion. */
CHECK (1, (v.i[0] == 0 && v.i[1] == FILL), "0", "%hhd", v.i);
CHECK (
6,
!memcmp_P (v.i, PVEC (1, 127, -128, -2, -1, -1), 6),
"1 127 128 255 32767 -2",
"%hhd %hhd %hhd %hhd %hhd %hhd",
v.i + 0, v.i + 1, v.i + 2,
v.i + 5, v.i + 4, v.i + 3);
/* All possible conversion types. */
CHECK (
8,
!memcmp_P (v.i, PVEC (1, 2, 3, 4, 5, 6, 7, 14, 8), 9),
"1 2 3 4 5 6 7 8",
"%hhd %hhu %hhi %hho %hhx %hhX %hhp %hhn %hhd",
v.i + 0, v.i + 1, v.i + 2, v.i + 3,
v.i + 4, v.i + 5, v.i + 6, v.i + 7,
v.i + 8);
/* Width field. */
CHECK (1, v.i[0] == 12 && v.i[1] == FILL, "123", "%2hhd", v.i);
CHECK (1, v.i[0] == 98 && v.i[1] == FILL, "00000000009876", "%12hhd", v.i);
/* A few conversions. */
CHECK (2, !memcmp_P (v.i, PVEC (1,2), 2) && (v.i[2] == FILL),
"1 2", "%hhd%hhd", v.i, v.i + 1);
CHECK (2, !memcmp_P (v.i, PVEC (1,3), 2) && (v.i[2] == FILL),
"1+3", "%hhd%hhd", v.i, v.i + 1);
CHECK (2, !memcmp_P (v.i, PVEC (1,-1), 2) && (v.i[2] == FILL),
"1-1", "%hhd%hhd", v.i, v.i + 1);
/* Suppress a writing. */
CHECK (0, (*(char *)v.i == FILL), "123", "%*hhd", v.i);
CHECK (2, !memcmp_P (v.i, PVEC (1,3), 2) && (v.i[2] == FILL),
"1 2 3", "%hhd%*hhd%hhd", v.i, v.i + 1);
return 0;
}
Car::Car(dWorldID world, dSpaceID space, CarDesignID n_car_design, AppDesignID n_app_design) :MyObject(world, space), car_design(n_car_design),
app_design(n_app_design)
{
int i, j, ind;
body_obj = 0;
for (i = 0; i < 2; ++i)
chain_obj[i] = 0;
for (i = 0; i < 6; ++i)
wheel_obj[i] = 0;
max_f = 0; // must be set with setMaxF
TrackDesignID track_design = car_design->left_track_design;
LinkDesignID link_design = track_design->link_design;
WheelDesignID wheel_design = car_design->wheel_design;
body_mass = car_design->getBodyMass();
// create body
createBody(car_design->getChassisPosition());
/*
for ( int i = 0 ; i < 6 ; ++i ) {
CreateToothedWheel(world, space,
wheel_obj[i], sprocket_teeth, R, t_sides, teeth_h_fuzz);
}
*/
// create sprocket wheels in back wheels position
for (j = 0, ind = FIRST_SPROCKET; j < 2; ++j, ++ind) {
wheel_obj[ind] = wheel_design->create(world, space);
sprocket[j] = wheel_obj[ind]->body[0];
const dReal *sprocket_pos =
track_design->getBackWheelPos();
PVEC(sprocket_pos);
dReal y = sprocket_pos[YY];
if (j == 1)
y += car_design->getTrackToTrack();
PEXP(y);
dBodySetPosition(sprocket[j], sprocket_pos[XX], y,
sprocket_pos[ZZ]);
}
// create sprockets and set their position
// create front wheels and set their position
for (j = 0, ind = FIRST_FRONT; j < 2; ++j, ++ind) {
wheel_obj[ind] = wheel_design->create(world, space);
front[j] = wheel_obj[ind]->body[0];
const dReal *front_pos = track_design->getFrontWheelPos();
PVEC(front_pos);
dReal y = front_pos[YY];
if (j == 1)
y += car_design->getTrackToTrack();
PEXP(y);
dBodySetPosition(front[j], front_pos[XX], y,
front_pos[ZZ]);
}
// create chains
chain_obj[0] =
new Chain(world, space, track_design, link_design);
track_design->moveDesign(0, car_design->getTrackToTrack(), 0); // instead of recreating design
chain_obj[1] = new Chain(world, space, track_design, link_design);
// create axle joints for sprockets
for (j = 0, ind = FIRST_SPROCKET; j < 2; ++j, ++ind) {
axle[ind] = dJointCreateHinge(world, 0);
dBodyID second = (body_obj ? body_obj->body[0] : 0);
dJointAttach(axle[ind], sprocket[j], second);
const dReal *sp_v = dBodyGetPosition(sprocket[j]);
const dReal *rot = dBodyGetRotation(sprocket[j]);
dJointSetHingeAnchor(axle[ind], sp_v[XX], sp_v[YY],
sp_v[ZZ]);
dJointSetHingeAxis(axle[ind], rot[1], rot[5], rot[9]);
}
/*
for ( j = 0, ind = FIRST_BACK ; j < 2 ; ++j, ++ind ) {
axle[ind] = dJointCreateHinge(world, 0); // no joint group (0==NULL)
dJointAttach(axle[ind], back[j], 0); // attach to world
const dReal* v = dBodyGetPosition(back[j]);
dJointSetHingeAnchor(axle[ind], v[XX], v[YY], v[ZZ]);
dJointSetHingeAxis(axle[ind], y_axis[XX], y_axis[YY], y_axis[ZZ]); // axis is up (positive z)
}
*/
// create front wheels joints
for (j = 0, ind = FIRST_FRONT; j < 2; ++j, ++ind) {
axle[ind] = dJointCreateHinge(world, 0);
dBodyID second = (body_obj ? body_obj->body[0] : 0);
dJointAttach(axle[ind], front[j], second);
const dReal *v = dBodyGetPosition(front[j]);
const dReal *rot = dBodyGetRotation(sprocket[j]);
dJointSetHingeAnchor(axle[ind], v[XX], v[YY], v[ZZ]);
dJointSetHingeAxis(axle[ind], rot[1], rot[5], rot[9]);
} if (body_obj)
body_obj->show_forces = 1;
// create appendage
if (body_obj && app_design) {
//app_obj = new App(world, space, app_design, body_obj->body[0]);
app_obj = 0;
};
// lets see what we have, height wise
if (chain_obj[0] != 0) {
const dReal *pos =
dBodyGetPosition(chain_obj[0]->body[0]);
std::cout << "debug: first link position, z wise\n";
PEXP(pos[ZZ]);
}
#ifdef ARE_YOU_NUTS_ABOUT_SPRINGS
// now create the suspension - it is between the wheels and the body.
Init(0, 2, 0);
// initially lets make some springs between the wheels and the body.
for (int i = 0 ; i < 2 ; ++i) {
dBody a_wheel = wheel_obj[i + FIRST_SPROCKET]->body[0];
};
#endif
}
