void dJointGetGearboxAxis2( dJointID j, dVector3 result )
{
dxJointGearbox* joint = static_cast<dxJointGearbox*>(j);
dUASSERT( joint, "bad joint argument" );
dBodyVectorToWorld(joint->node[0].body,
joint->axis2[0], joint->axis2[1], joint->axis2[2],
result);
}
void
dxJointGearbox::getInfo2( dxJoint::Info2* info )
{
dVector3 globalAxis1, globalAxis2;
dBodyVectorToWorld(node[0].body, axis1[0], axis1[1], axis1[2], globalAxis1);
dBodyVectorToWorld(node[1].body, axis2[0], axis2[1], axis2[2], globalAxis2);
double ang1 = getHingeAngle(refBody1,node[0].body,globalAxis1,qrel1);
double ang2 = getHingeAngle(refBody2,node[1].body,globalAxis2,qrel2);
// printf("a1(%f) a10(%f) a2(%f) a20(%f)\n",
// ang1, cumulative_angle1, ang2, cumulative_angle2);
cumulative_angle1 = dShortestAngularDistanceUpdate(cumulative_angle1,ang1);
cumulative_angle2 = dShortestAngularDistanceUpdate(cumulative_angle2,ang2);
double err = dShortestAngularDistance(
cumulative_angle1, -ratio * cumulative_angle2);
// FIXME: error calculation is not amenable to reset of poses,
// cumulative angles might snap to wrong angular value.
// printf("a1(%f) a1f(%f) a2(%f) a2f(%f) e(%f)\n",
// ang1, cumulative_angle1, ang2, cumulative_angle2, err);
info->J1a[0] = globalAxis1[0];
info->J1a[1] = globalAxis1[1];
info->J1a[2] = globalAxis1[2];
info->J2a[0] = ratio * globalAxis2[0];
info->J2a[1] = ratio * globalAxis2[1];
info->J2a[2] = ratio * globalAxis2[2];
dReal k = info->fps * info->erp;
info->c[0] = -k * err;
// dVector3 d;
// dAddScaledVectors3(d, node[0].body->avel, node[1].body->avel,
// 1.0, ratio);
// printf("d: %f\n", dCalcVectorDot3(globalAxis1, d));
}
void dJointGetTransmissionAxis2( dJointID j, dVector3 result )
{
dxJointTransmission* joint = static_cast<dxJointTransmission*>(j);
dUASSERT( joint, "bad joint argument" );
dUASSERT( result, "bad result argument" );
if (joint->node[1].body) {
dBodyVectorToWorld(joint->node[1].body,
joint->axes[1][0],
joint->axes[1][1],
joint->axes[1][2],
result);
}
}
void dJointGetTransmissionAxis( dJointID j, dVector3 result )
{
dxJointTransmission* joint = static_cast<dxJointTransmission*>(j);
dUASSERT( joint, "bad joint argument" );
dUASSERT( result, "bad result argument" );
dUASSERT(joint->mode == dTransmissionParallelAxes,
"axes must be queried individualy in current mode" );
if (joint->node[0].body) {
dBodyVectorToWorld(joint->node[0].body,
joint->axes[0][0],
joint->axes[0][1],
joint->axes[0][2],
result);
}
}
void Buggy::doDynamics(dBodyID body)
{
//dReal *v = (dReal *)dBodyGetLinearVel(body);
//dJointSetHinge2Param (carjoint[0],dParamVel,getThrottle());
//dJointSetHinge2Param (carjoint[0],dParamFMax,1000);
dJointSetHinge2Param (carjoint[0],dParamVel2,yRotAngle);
dJointSetHinge2Param (carjoint[0],dParamFMax2,1000);
dJointSetHinge2Param (carjoint[1],dParamVel2,yRotAngle);
dJointSetHinge2Param (carjoint[1],dParamFMax2,1000);
// steering
dReal v = dJointGetHinge2Angle1 (carjoint[0]);
if (v > 0.1) v = 0.1;
if (v < -0.1) v = -0.1;
v *= 10.0;
//dJointSetHinge2Param (carjoint[0],dParamVel,0);
//dJointSetHinge2Param (carjoint[0],dParamFMax,1000);
//dJointSetHinge2Param (carjoint[0],dParamLoStop,-0.75);
//dJointSetHinge2Param (carjoint[0],dParamHiStop,0.75);
//dJointSetHinge2Param (carjoint[0],dParamFudgeFactor,0.1);
//dBodyAddRelForce (body,0, 0,getThrottle());
// This should be after the world step
/// stuff
dVector3 result;
dBodyVectorToWorld(body, 0,0,1,result);
setForward(result[0],result[1],result[2]);
const dReal *dBodyPosition = dBodyGetPosition(body);
const dReal *dBodyRotation = dBodyGetRotation(body);
setPos(dBodyPosition[0],dBodyPosition[1],dBodyPosition[2]);
setLocation((float *)dBodyPosition, (float *)dBodyRotation);
}
void
dxJointDBall::getInfo2( dxJoint::Info2 *info )
{
info->erp = erp;
info->cfm[0] = cfm;
dVector3 globalA1, globalA2;
dBodyGetRelPointPos(node[0].body, anchor1[0], anchor1[1], anchor1[2], globalA1);
if (node[1].body)
dBodyGetRelPointPos(node[1].body, anchor2[0], anchor2[1], anchor2[2], globalA2);
else
dCopyVector3(globalA2, anchor2);
dVector3 q;
dSubtractVectors3(q, globalA1, globalA2);
#ifdef dSINGLE
const dReal MIN_LENGTH = REAL(1e-7);
#else
const dReal MIN_LENGTH = REAL(1e-12);
#endif
if (dCalcVectorLength3(q) < MIN_LENGTH) {
// too small, let's choose an arbitrary direction
// heuristic: difference in velocities at anchors
dVector3 v1, v2;
dBodyGetPointVel(node[0].body, globalA1[0], globalA1[1], globalA1[2], v1);
if (node[1].body)
dBodyGetPointVel(node[1].body, globalA2[0], globalA2[1], globalA2[2], v2);
else
dSetZero(v2, 3);
dSubtractVectors3(q, v1, v2);
if (dCalcVectorLength3(q) < MIN_LENGTH) {
// this direction is as good as any
q[0] = 1;
q[1] = 0;
q[2] = 0;
}
}
dNormalize3(q);
info->J1l[0] = q[0];
info->J1l[1] = q[1];
info->J1l[2] = q[2];
dVector3 relA1;
dBodyVectorToWorld(node[0].body,
anchor1[0], anchor1[1], anchor1[2],
relA1);
dMatrix3 a1m;
dSetZero(a1m, 12);
dSetCrossMatrixMinus(a1m, relA1, 4);
dMultiply1_331(info->J1a, a1m, q);
if (node[1].body) {
info->J2l[0] = -q[0];
info->J2l[1] = -q[1];
info->J2l[2] = -q[2];
dVector3 relA2;
dBodyVectorToWorld(node[1].body,
anchor2[0], anchor2[1], anchor2[2],
relA2);
dMatrix3 a2m;
dSetZero(a2m, 12);
dSetCrossMatrixPlus(a2m, relA2, 4);
dMultiply1_331(info->J2a, a2m, q);
}
const dReal k = info->fps * info->erp;
info->c[0] = k * (targetDistance - dCalcPointsDistance3(globalA1, globalA2));
}
void
dxJointTransmission::getInfo2( dReal worldFPS,
dReal /*worldERP*/,
const Info2Descr* info )
{
dVector3 a[2], n[2], l[2], r[2], c[2], s, t, O, d, z, u, v;
dReal theta, delta, nn, na_0, na_1, cosphi, sinphi, m;
const dReal *p[2], *omega[2];
int i;
// Transform all needed quantities to the global frame.
for (i = 0 ; i < 2 ; i += 1) {
dBodyGetRelPointPos(node[i].body,
anchors[i][0], anchors[i][1], anchors[i][2],
a[i]);
dBodyVectorToWorld(node[i].body, axes[i][0], axes[i][1], axes[i][2],
n[i]);
p[i] = dBodyGetPosition(node[i].body);
omega[i] = dBodyGetAngularVel(node[i].body);
}
if (update) {
// Make sure both gear reference frames end up with the same
// handedness.
if (dCalcVectorDot3(n[0], n[1]) < 0) {
dNegateVector3(axes[0]);
dNegateVector3(n[0]);
}
}
// Calculate the mesh geometry based on the current mode.
switch (mode) {
case dTransmissionParallelAxes:
// Simply calculate the contact point as the point on the
// baseline that will yield the correct ratio.
dIASSERT (ratio > 0);
dSubtractVectors3(d, a[1], a[0]);
dAddScaledVectors3(c[0], a[0], d, 1, ratio / (1 + ratio));
dCopyVector3(c[1], c[0]);
dNormalize3(d);
for (i = 0 ; i < 2 ; i += 1) {
dCalcVectorCross3(l[i], d, n[i]);
}
break;
case dTransmissionIntersectingAxes:
// Calculate the line of intersection between the planes of the
// gears.
dCalcVectorCross3(l[0], n[0], n[1]);
dCopyVector3(l[1], l[0]);
nn = dCalcVectorDot3(n[0], n[1]);
dIASSERT(fabs(nn) != 1);
na_0 = dCalcVectorDot3(n[0], a[0]);
na_1 = dCalcVectorDot3(n[1], a[1]);
dAddScaledVectors3(O, n[0], n[1],
(na_0 - na_1 * nn) / (1 - nn * nn),
(na_1 - na_0 * nn) / (1 - nn * nn));
// Find the contact point as:
//
// c = ((r_a - O) . l) l + O
//
// where r_a the anchor point of either gear and l, O the tangent
// line direction and origin.
for (i = 0 ; i < 2 ; i += 1) {
dSubtractVectors3(d, a[i], O);
m = dCalcVectorDot3(d, l[i]);
dAddScaledVectors3(c[i], O, l[i], 1, m);
}
break;
case dTransmissionChainDrive:
dSubtractVectors3(d, a[0], a[1]);
m = dCalcVectorLength3(d);
dIASSERT(m > 0);
// Caclulate the angle of the contact point relative to the
// baseline.
cosphi = clamp((radii[1] - radii[0]) / m, REAL(-1.0), REAL(1.0)); // Force into range to fix possible computation errors
sinphi = dSqrt (REAL(1.0) - cosphi * cosphi);
dNormalize3(d);
for (i = 0 ; i < 2 ; i += 1) {
// Calculate the contact radius in the local reference
// frame of the chain. This has axis x pointing along the
// baseline, axis y pointing along the sprocket axis and
// the remaining axis normal to both.
u[0] = radii[i] * cosphi;
u[1] = 0;
u[2] = radii[i] * sinphi;
// Transform the contact radius into the global frame.
dCalcVectorCross3(z, d, n[i]);
v[0] = dCalcVectorDot3(d, u);
v[1] = dCalcVectorDot3(n[i], u);
v[2] = dCalcVectorDot3(z, u);
// Finally calculate contact points and l.
dAddVectors3(c[i], a[i], v);
dCalcVectorCross3(l[i], v, n[i]);
dNormalize3(l[i]);
// printf ("%d: %f, %f, %f\n",
// i, l[i][0], l[i][1], l[i][2]);
}
break;
}
if (update) {
// We need to calculate an initial reference frame for each
// wheel which we can measure the current phase against. This
// frame will have the initial contact radius as the x axis,
// the wheel axis as the z axis and their cross product as the
// y axis.
for (i = 0 ; i < 2 ; i += 1) {
dSubtractVectors3 (r[i], c[i], a[i]);
radii[i] = dCalcVectorLength3(r[i]);
dIASSERT(radii[i] > 0);
dBodyVectorFromWorld(node[i].body, r[i][0], r[i][1], r[i][2],
reference[i]);
dNormalize3(reference[i]);
dCopyVector3(reference[i] + 8, axes[i]);
dCalcVectorCross3(reference[i] + 4, reference[i] + 8, reference[i]);
// printf ("%f\n", dDOT(r[i], n[i]));
// printf ("(%f, %f, %f,\n %f, %f, %f,\n %f, %f, %f)\n",
// reference[i][0],reference[i][1],reference[i][2],
// reference[i][4],reference[i][5],reference[i][6],
// reference[i][8],reference[i][9],reference[i][10]);
radii[i] = radii[i];
phase[i] = 0;
}
ratio = radii[0] / radii[1];
update = 0;
}
for (i = 0 ; i < 2 ; i += 1) {
dReal phase_hat;
dSubtractVectors3 (r[i], c[i], a[i]);
// Transform the (global) contact radius into the gear's
// reference frame.
dBodyVectorFromWorld (node[i].body, r[i][0], r[i][1], r[i][2], s);
dMultiply0_331(t, reference[i], s);
// Now simply calculate its angle on the plane relative to the
// x-axis which is the initial contact radius. This will be
// an angle between -pi and pi that is coterminal with the
// actual phase of the wheel. To find the real phase we
// estimate it by adding omega * dt to the old phase and then
// find the closest angle to that, that is coterminal to
// theta.
theta = atan2(t[1], t[0]);
phase_hat = phase[i] + dCalcVectorDot3(omega[i], n[i]) / worldFPS;
if (phase_hat > M_PI_2) {
if (theta < 0) {
theta += (dReal)(2 * M_PI);
}
theta += (dReal)(floor(phase_hat / (2 * M_PI)) * (2 * M_PI));
} else if (phase_hat < -M_PI_2) {
if (theta > 0) {
theta -= (dReal)(2 * M_PI);
}
theta += (dReal)(ceil(phase_hat / (2 * M_PI)) * (2 * M_PI));
}
if (phase_hat - theta > M_PI) {
phase[i] = theta + (dReal)(2 * M_PI);
} else if (phase_hat - theta < -M_PI) {
phase[i] = theta - (dReal)(2 * M_PI);
} else {
phase[i] = theta;
}
dIASSERT(fabs(phase_hat - phase[i]) < M_PI);
}
// Calculate the phase error. Depending on the mode the condition
// is that the distances traveled by each contact point must be
// either equal (chain and sprockets) or opposite (gears).
if (mode == dTransmissionChainDrive) {
delta = (dCalcVectorLength3(r[0]) * phase[0] -
dCalcVectorLength3(r[1]) * phase[1]);
} else {
delta = (dCalcVectorLength3(r[0]) * phase[0] +
dCalcVectorLength3(r[1]) * phase[1]);
}
// When in chain mode a torque reversal, signified by the change
// in sign of the wheel phase difference, has the added effect of
// switching the active chain branch. We must therefore reflect
// the contact points and tangents across the baseline.
if (mode == dTransmissionChainDrive && delta < 0) {
dVector3 d;
dSubtractVectors3(d, a[0], a[1]);
for (i = 0 ; i < 2 ; i += 1) {
dVector3 nn;
dReal a;
dCalcVectorCross3(nn, n[i], d);
a = dCalcVectorDot3(nn, nn);
dIASSERT(a > 0);
dAddScaledVectors3(c[i], c[i], nn,
1, -2 * dCalcVectorDot3(c[i], nn) / a);
dAddScaledVectors3(l[i], l[i], nn,
-1, 2 * dCalcVectorDot3(l[i], nn) / a);
}
}
// Do not add the constraint if there's backlash and we're in the
// backlash gap.
if (backlash == 0 || fabs(delta) > backlash) {
// The constraint is satisfied iff the absolute velocity of the
// contact point projected onto the tangent of the wheels is equal
// for both gears. This velocity can be calculated as:
//
// u = v + omega x r_c
//
// The constraint therefore becomes:
// (v_1 + omega_1 x r_c1) . l = (v_2 + omega_2 x r_c2) . l <=>
// (v_1 . l + (r_c1 x l) . omega_1 = v_2 . l + (r_c2 x l) . omega_2
for (i = 0 ; i < 2 ; i += 1) {
dSubtractVectors3 (r[i], c[i], p[i]);
}
dCalcVectorCross3(info->J1a, r[0], l[0]);
dCalcVectorCross3(info->J2a, l[1], r[1]);
dCopyVector3(info->J1l, l[0]);
dCopyNegatedVector3(info->J2l, l[1]);
if (delta > 0) {
if (backlash > 0) {
info->lo[0] = -dInfinity;
info->hi[0] = 0;
}
info->c[0] = -worldFPS * erp * (delta - backlash);
} else {
if (backlash > 0) {
info->lo[0] = 0;
info->hi[0] = dInfinity;
}
info->c[0] = -worldFPS * erp * (delta + backlash);
}
}
info->cfm[0] = cfm;
// printf ("%f, %f, %f, %f, %f\n", delta, phase[0], phase[1], -phase[1] / phase[0], ratio);
// Cache the contact point (in world coordinates) to avoid
// recalculation if requested by the user.
dCopyVector3(contacts[0], c[0]);
dCopyVector3(contacts[1], c[1]);
}
//collide camera with track, generate acceleration on camera if collisding
void camera_physics_step()
{
//some values that are easy to deal with:
dReal time = internal.stepsize;
car_struct *car = camera.car;
camera_settings *settings = camera.settings;
//if camera got a targeted car and proper settings, simulate movment
//
//divided into 4 parts:
//1) calculate velocity
//2) check for collisions
//3) add damping to velocity
//4) move camera
//
if (car && settings)
{
//random values will come handy:
//check for some exceptions
if (settings->reverse) //enabled
{
if (car->throttle > 0.0) //wanting to go forward
camera.reverse = false;
else if (car->throttle < 0.0 && car->velocity < 0.0) //wanting and going backwards
camera.reverse = true;
}
if (settings->in_air) //in air enabled
{
if (!(car->sensor1->event) && !(car->sensor2->event)) //in air
{
if (camera.in_air) //in ground mode
{
//smooth transition between offset and center (and needed)
if (settings->offset_scale_speed != 0 && camera.offset_scale > 0)
camera.offset_scale -= (settings->offset_scale_speed*time);
else //jump directly
camera.offset_scale = 0;
}
if (!camera.in_air) //camera not in "air mode"
{
if (camera.air_timer > settings->air_time)
{
camera.in_air = true; //go to air mode
camera.air_timer = 0; //reset timer
}
else
camera.air_timer += time;
}
}
else //not in air
{
if (camera.in_air) //camera in "air mode"
{
if (camera.air_timer > settings->ground_time)
{
camera.in_air = false; //leave air mode
camera.air_timer = 0; //reset timer
}
else
camera.air_timer += time;
}
else //camera in "ground mode"
{
//smooth transition between center and offset (and needed)
if (settings->offset_scale_speed != 0 && camera.offset_scale < 1)
camera.offset_scale += (settings->offset_scale_speed*time);
else //jump directly
camera.offset_scale = 1;
}
}
}
//store old velocity
dReal old_vel[3] = {camera.vel[0], camera.vel[1], camera.vel[2]};
//wanted position of "target" - position on car that should be focused
dVector3 t_pos;
//wanted position of camera relative to anchor (translated to world coords)
dVector3 pos_wanted;
if (camera.reverse && !camera.in_air) //move target and position to opposite side (if not just spinning in air)
{
dBodyGetRelPointPos (car->bodyid, settings->target[0], -settings->target[1], settings->target[2]*car->dir, t_pos);
dBodyVectorToWorld(car->bodyid, settings->distance[0], -settings->distance[1], settings->distance[2]*car->dir, pos_wanted);
}
else //normal
{
dBodyGetRelPointPos (car->bodyid, settings->target[0]*camera.offset_scale,
settings->target[1]*camera.offset_scale, settings->target[2]*car->dir*camera.offset_scale, t_pos);
dBodyVectorToWorld(car->bodyid, settings->distance[0], settings->distance[1], settings->distance[2]*car->dir, pos_wanted);
}
//position and velocity of anchor
dVector3 a_pos;
dBodyGetRelPointPos (car->bodyid, settings->anchor[0], settings->anchor[1], settings->anchor[2]*car->dir, a_pos);
//relative pos and vel of camera (from anchor)
dReal pos[3] = {camera.pos[0]-a_pos[0], camera.pos[1]-a_pos[1], camera.pos[2]-a_pos[2]};
//vector lengths
dReal pos_l = v_length(pos[0], pos[1], pos[2]);
//how far from car we want to stay
//(TODO: could be computed just once - only when changing camera)
dReal pos_wanted_l = v_length(pos_wanted[0], pos_wanted[1], pos_wanted[2]);
//unit vectors
dReal pos_u[3] = {pos[0]/pos_l, pos[1]/pos_l, pos[2]/pos_l};
dReal pos_wanted_u[3] = {pos_wanted[0]/pos_wanted_l, pos_wanted[1]/pos_wanted_l, pos_wanted[2]/pos_wanted_l};
//
// 1) spring physics for calculating acceleration
//
//"linear spring" between anchor and camera (based on distance)
dReal dist = pos_l-pos_wanted_l;
if (settings->linear_stiffness == 0) //disabled smooth movement, jump directly
{
//chanses are we have an anchor distance of 0, then vel=0
if (pos_wanted_l == 0)
{
//position at wanted
camera.pos[0]=a_pos[0];
camera.pos[1]=a_pos[1];
camera.pos[2]=a_pos[2];
//velocity 0
camera.vel[0]=0;
camera.vel[1]=0;
camera.vel[2]=0;
}
else
{
//set position
camera.pos[0]-=pos_u[0]*dist;
camera.pos[1]-=pos_u[1]*dist;
camera.pos[2]-=pos_u[2]*dist;
//velocity towards/from anchor = 0
//vel towards anchor
dReal dot = (pos_u[0]*camera.vel[0] + pos_u[1]*camera.vel[1] + pos_u[2]*camera.vel[2]);
//remove vel towards anchor
camera.vel[0]-=pos_u[0]*dot;
camera.vel[1]-=pos_u[1]*dot;
camera.vel[2]-=pos_u[2]*dot;
}
}
else //smooth movement
{
//how much acceleration (based on distance from wanted distance)
dReal acceleration = time*(camera.settings->linear_stiffness)*dist;
camera.vel[0]-=pos_u[0]*acceleration;
camera.vel[1]-=pos_u[1]*acceleration;
camera.vel[2]-=pos_u[2]*acceleration;
}
//perpendicular "angular spring" to move camera behind car
if (pos_wanted_l > 0 && !camera.in_air) //actually got distance, and camera not in "air mode"
{
//dot between wanted and current rotation
dReal dot = (pos_wanted_u[0]*pos_u[0] + pos_wanted_u[1]*pos_u[1] + pos_wanted_u[2]*pos_u[2]);
if (dot < 1.0) //if we aren't exactly at wanted position (and prevent possibility of acos a number bigger than 1.0)
{
//angle
dReal angle = acos(dot);
//how much acceleration
dReal accel = time*angle*(settings->angular_stiffness);
//direction of acceleration (remove part of wanted that's along current pos)
dReal dir[3];
dir[0]=pos_wanted_u[0]-dot*pos_u[0];
dir[1]=pos_wanted_u[1]-dot*pos_u[1];
dir[2]=pos_wanted_u[2]-dot*pos_u[2];
//not unit, get length and modify accel to compensate for not unit
accel /= v_length(dir[0], dir[1], dir[2]);
camera.vel[0]+=(accel*dir[0]);
camera.vel[1]+=(accel*dir[1]);
camera.vel[2]+=(accel*dir[2]);
}
}
//
// 2) check for collision, and if so, remove possible movement into collision direction
//
if (settings->radius > 0)
{
dGeomID geom = dCreateSphere (0, settings->radius);
dGeomSetPosition(geom, camera.pos[0], camera.pos[1], camera.pos[2]);
dContactGeom contact[internal.contact_points];
int count = dCollide ( (dGeomID)(track.object->space), geom, internal.contact_points, &contact[0], sizeof(dContactGeom));
int i;
dReal depth;
dReal V;
for (i=0; i<count; ++i)
{
depth = contact[i].depth;
camera.pos[0]-=contact[i].normal[0]*depth;
camera.pos[1]-=contact[i].normal[1]*depth;
camera.pos[2]-=contact[i].normal[2]*depth;
//remove movement into colliding object
//velocity along collision axis
V = camera.vel[0]*contact[i].normal[0] + camera.vel[1]*contact[i].normal[1] + camera.vel[2]*contact[i].normal[2];
if (V > 0) //right direction (not away from collision)?
{
//remove direction
camera.vel[0]-=V*contact[i].normal[0];
camera.vel[1]-=V*contact[i].normal[1];
camera.vel[2]-=V*contact[i].normal[2];
}
}
dGeomDestroy (geom);
}
//
// 3) damping of current velocity
//
if (settings->relative_damping)
{
//damping (of relative movement)
dVector3 a_vel; //anchor velocity
dBodyGetRelPointVel (car->bodyid, settings->anchor[0], settings->anchor[1], settings->anchor[2]*car->dir, a_vel);
dReal vel[3] = {camera.vel[0]-a_vel[0], camera.vel[1]-a_vel[1], camera.vel[2]-a_vel[2]}; //velocity relative to anchor
dReal damping = (time*settings->damping);
if (damping > 1)
damping=1;
camera.vel[0]-=damping*vel[0];
camera.vel[1]-=damping*vel[1];
camera.vel[2]-=damping*vel[2];
}
else
{
//absolute damping
dReal damping = 1-(time*settings->damping);
if (damping < 0)
damping=0;
camera.vel[0]*=damping;
camera.vel[1]*=damping;
camera.vel[2]*=damping;
}
//
// 4) movement
//
//during the step, camera will have linear acceleration from old velocity to new
//avarge velocity over the step is between new and old velocity
camera.pos[0]+=((camera.vel[0]+old_vel[0])/2)*time;
camera.pos[1]+=((camera.vel[1]+old_vel[1])/2)*time;
camera.pos[2]+=((camera.vel[2]+old_vel[2])/2)*time;
//movement of camera done.
//
//the following is smooth rotation and focusing
//
//smooth rotation (if enabled)
//(move partially from current "up" to car "up", and make unit)
dReal target_up[3];
if (camera.in_air) //if in air, use absolute up instead
{
target_up[0] = 0;
target_up[1] = 0;
target_up[2] = 1;
}
else //use car up
{
const dReal *rotation = dBodyGetRotation (car->bodyid);
target_up[0] = rotation[2]*car->dir;
target_up[1] = rotation[6]*car->dir;
target_up[2] = rotation[10]*car->dir;
}
if (settings->rotation_tightness == 0) //disabled, rotate directly
{
camera.up[0]=target_up[0];
camera.up[1]=target_up[1];
camera.up[2]=target_up[2];
}
else
{
dReal diff[3]; //difference between
diff[0]=target_up[0]-camera.up[0];
diff[1]=target_up[1]-camera.up[1];
diff[2]=target_up[2]-camera.up[2];
dReal movement=time*(settings->rotation_tightness);
if (movement > 1)
movement=1;
camera.up[0]+=diff[0]*movement;
camera.up[1]+=diff[1]*movement;
camera.up[2]+=diff[2]*movement;
//gluLookAt wants up to be unit
dReal length=v_length(camera.up[0], camera.up[1], camera.up[2]);
camera.up[0]/=length;
camera.up[1]/=length;
camera.up[2]/=length;
}
//smooth movement of target focus (if enabled)
if (settings->target_tightness == 0)
{
camera.t_pos[0] = t_pos[0];
camera.t_pos[1] = t_pos[1];
camera.t_pos[2] = t_pos[2];
}
else
{
dReal diff[3], movement;
diff[0]=t_pos[0]-camera.t_pos[0];
diff[1]=t_pos[1]-camera.t_pos[1];
diff[2]=t_pos[2]-camera.t_pos[2];
movement = time*(settings->target_tightness);
if (movement>1)
movement=1;
camera.t_pos[0]+=diff[0]*movement;
camera.t_pos[1]+=diff[1]*movement;
camera.t_pos[2]+=diff[2]*movement;
}
}
}
// called by Webots at the beginning of the simulation
void webots_physics_init(dWorldID w, dSpaceID s, dJointGroupID j) {
int i;
// store global objects for later use
world = w;
space = s;
contact_joint_group = j;
// get floor geometry
floor_geom = getGeom(floor_name);
if (!floor_geom)
return;
// get foot geometry and body id's
for (i = 0; i < N_FEET; i++) {
foot_geom[i] = getGeom(foot_name[i]);
if (!foot_geom[i])
return;
foot_body[i] = dGeomGetBody(foot_geom[i]);
if (!foot_body[i])
return;
}
// create universal joints for linear actuators
for (i = 0; i < 10; i++) {
dBodyID upper_piston = getBody(upper_piston_name[i]);
dBodyID lower_piston = getBody(lower_piston_name[i]);
dBodyID upper_link = getBody(upper_link_name[i]);
dBodyID lower_link = getBody(lower_link_name[i]);
if (!upper_piston || !lower_piston || !upper_link || !lower_link)
return;
// create a ball and socket joint (3 DOFs) to attach the lower piston body to the lower link
// we don't need a universal joint here, because the piston's passive rotation is prevented
// by the universal joint at its upper end.
dJointID lower_balljoint = dJointCreateBall(world, 0);
dJointAttach(lower_balljoint, lower_piston, lower_link);
// transform attachement point from local to global coordinate system
// warning: this is a hard-coded translation
dVector3 lower_ball;
dBodyGetRelPointPos(lower_piston, 0, 0, -0.075, lower_ball);
// set attachement point (anchor)
dJointSetBallAnchor(lower_balljoint, lower_ball[0], lower_ball[1], lower_ball[2]);
// create a universal joint (2 DOFs) to attach upper piston body to upper link
// we need to use a universal joint to prevent the piston from passively rotating around its long axis
dJointID upper_ujoint = dJointCreateUniversal(world, 0);
dJointAttach(upper_ujoint, upper_piston, upper_link);
// transform attachement point from local to global coordinate system
// warning: this is a hard-coded translation
dVector3 upper_ball;
dBodyGetRelPointPos(upper_piston, 0, 0, 0, upper_ball);
// set attachement point (anchor)
dJointSetUniversalAnchor(upper_ujoint, upper_ball[0], upper_ball[1], upper_ball[2]);
// set the universal joint axes
dVector3 upper_xaxis;
dVector3 upper_yaxis;
dBodyVectorToWorld(upper_piston, 1, 0, 0, upper_xaxis);
dBodyVectorToWorld(upper_piston, 0, 1, 0, upper_yaxis);
dJointSetUniversalAxis1(upper_ujoint, upper_xaxis[0], upper_xaxis[1], upper_xaxis[2]);
dJointSetUniversalAxis2(upper_ujoint, upper_yaxis[0], upper_yaxis[1], upper_yaxis[2]);
}
}
void SimpleTrackedVehicleEnvironment::nearCallbackGrouserTerrain(dGeomID o1, dGeomID o2) {
dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
if(b1 && b2 && dAreConnectedExcluding(b1, b2, dJointTypeContact)) return;
// body of the whole vehicle
dBodyID vehicleBody = ((SimpleTrackedVehicle*)this->v)->vehicleBody;
unsigned long geom1Categories = dGeomGetCategoryBits(o1);
// speeds of the belts
const dReal leftBeltSpeed = ((SimpleTrackedVehicle*)this->v)->leftTrack->getVelocity();
const dReal rightBeltSpeed = ((SimpleTrackedVehicle*)this->v)->rightTrack->getVelocity();
dReal beltSpeed = 0; // speed of the belt which is in collision and examined right now
if (geom1Categories & Category::LEFT) {
beltSpeed = leftBeltSpeed;
} else {
beltSpeed = rightBeltSpeed;
}
// the desired linear and angular speeds (set by desired track velocities)
const dReal linearSpeed = (leftBeltSpeed + rightBeltSpeed) / 2;
const dReal angularSpeed = (leftBeltSpeed - rightBeltSpeed) * steeringEfficiency / tracksDistance;
// radius of the turn the robot is doing
const dReal desiredRotationRadiusSigned = (fabs(angularSpeed) < 0.1) ?
dInfinity : // is driving straight
((fabs(linearSpeed) < 0.1) ?
0 : // is rotating about a single point
linearSpeed / angularSpeed // general movement
);
dVector3 yAxisGlobal; // vector pointing from vehicle body center in the direction of +y axis
dVector3 centerOfRotation; // at infinity if driving straight, so we need to distinguish the case
{ // compute the center of rotation
dBodyVectorToWorld(vehicleBody, 0, 1, 0, yAxisGlobal);
dCopyVector3(centerOfRotation, yAxisGlobal);
// make the unit vector as long as we need (and change orientation if needed; the radius is a signed number)
dScaleVector3(centerOfRotation, desiredRotationRadiusSigned);
const dReal *vehicleBodyPos = dBodyGetPosition(vehicleBody);
dAddVectors3(centerOfRotation, centerOfRotation, vehicleBodyPos);
}
int maxContacts = 20;
dContact contact[maxContacts];
int numContacts = dCollide(o1, o2, maxContacts, &contact[0].geom, sizeof(dContact));
for(size_t i = 0; i < numContacts; i++) {
dVector3 contactInVehiclePos; // position of the contact point relative to vehicle body
dBodyGetPosRelPoint(vehicleBody, contact[i].geom.pos[0], contact[i].geom.pos[1], contact[i].geom.pos[2], contactInVehiclePos);
dVector3 beltDirection; // vector tangent to the belt pointing in the belt's movement direction
dCalcVectorCross3(beltDirection, contact[i].geom.normal, yAxisGlobal);
if (beltSpeed > 0) {
dNegateVector3(beltDirection);
}
if (desiredRotationRadiusSigned != dInfinity) { // non-straight drive
dVector3 COR2Contact; // vector pointing from the center of rotation to the contact point
dSubtractVectors3(COR2Contact, contact[i].geom.pos, centerOfRotation);
// the friction force should be perpendicular to COR2Contact
dCalcVectorCross3(contact[i].fdir1, contact[i].geom.normal, COR2Contact);
const dReal linearSpeedSignum = (fabs(linearSpeed) > 0.1) ? sgn(linearSpeed) : 1;
// contactInVehiclePos[0] > 0 means the contact is in the front part of the track
if (sgn(angularSpeed) * sgn(dCalcVectorDot3(yAxisGlobal, contact[i].fdir1)) !=
sgn(contactInVehiclePos[0]) * linearSpeedSignum) {
dNegateVector3(contact[i].fdir1);
}
} else { // straight drive
dCalcVectorCross3(contact[i].fdir1, contact[i].geom.normal, yAxisGlobal);
if (dCalcVectorDot3(contact[i].fdir1, beltDirection) < 0) {
dNegateVector3(contact[i].fdir1);
}
}
// use friction direction and motion1 to simulate the track movement
contact[i].surface.mode = dContactFDir1 | dContactMotion1 | dContactMu2;
contact[i].surface.mu = 0.5;
contact[i].surface.mu2 = 10;
// the dot product <beltDirection,fdir1> is the cosine of the angle they form (because both are unit vectors)
contact[i].surface.motion1 = -dCalcVectorDot3(beltDirection, contact[i].fdir1) * fabs(beltSpeed) * 0.07;
// friction force visualization
dMatrix3 forceRotation;
dVector3 vec;
dBodyVectorToWorld(vehicleBody, 1, 0, 0, vec);
dRFrom2Axes(forceRotation, contact[i].fdir1[0], contact[i].fdir1[1], contact[i].fdir1[2], vec[0], vec[1], vec[2]);
posr data;
dCopyVector3(data.pos, contact[i].geom.pos);
dCopyMatrix4x3(data.R, forceRotation);
forces.push_back(data);
dJointID c = dJointCreateContact(this->world, this->contactGroup, &contact[i]);
dJointAttach(c, b1, b2);
if(!isValidCollision(o1, o2, contact[i]))
this->badCollision = true;
if(config.contact_grouser_terrain.debug)
this->contacts.push_back(contact[i].geom);
}
}
void PhysicsBody::vectorToWorld(const Vec3f &v, Vec3f &result)
{
dVector3 t;
dBodyVectorToWorld(_BodyID, v.x(), v.y(), v.z(), t);
result.setValue(Vec3f(t[0], t[1], t[2]));
}
void dxPlanarJoint::getInfo2(dReal worldFPS, dReal worldERP, const Info2Descr* info)
{
/* ====================================================================
* This joint restricts 2 rotational and 1 translational DOF.
*
* The 2 rotational constraints are essentially the same as in dxJointPiston, so the implementation here is mostly
* just a copy of them.
*
* The translational 1DOF constraint has not yet been implemented for any joint in ODE, so we'll talk more about it.
* It is similar to the constraints in a slider joint, but in this case we only want body2 to move the plane, and
* body1 anchor has no use in this case, too.
*
* We basically want to express the plane in body2 local coordinates (because it should move along with body2), and
* check, if body1 origin lies in the plane.
*
* Let's say n' is the plane normal in body2 coordinates and a' is the anchor point in body2 coordinates
* (with n, a, being the corresponding global variables).
* Further, let's call the global position of body1 p1, and similarly p2 for body2.
* Last, let's call body2 rotation matrix R2 (so that n = R2*n' and a = R2*a' + p2).
*
* The plane can then be expressed in global coordinates as:
* (n^ . x) - (n^ . a) = 0,
* where x is in global coordinates and ^ denotes vector/matrix transpose.
*
* Rewriting the equation using body2 local coordinates and setting x=p1, we get the constraint:
* (R2*n')^ * p1 - (R2*n')^ * (R2*a' + p2) = 0
* ...
* (n'^ * R2^ * p1) - (n'^ * R2^ * p2) - (n'^ * a') = 0
* (n^ * p1) - (n^ * p2) - (n'^ * a') = 0
*
* The part left to "=" will be the "c" on the RHS of the Jacobian equation (because it expresses
* the distance of p1 from the plane).
*
* Next, we need to take time derivative of the constraint to get the J1 and J2 parts.
* v1, v2, w1 and w2 denote the linear and angular velocities od body1 and body2.
* [a]x denotes the skew-symmetric cross-product matrix of vector a.
*
* d/dt [(n'^ * R2^ * p1) - (n'^ * R2^ * p2) - (n'^ * a') = 0] (n', a' are constant in time)
* n'^ * ((d/dt[R2^] * p1) + (R2^ * v1)) - n'^ * ((d/dt[R2^] * p2) + (R2^ * v2)) = 0
* n'^ * ((([w2]x R2)^ * p1) + (R2^ * v1)) - n'^ * ((([w2]x R2)^ * p2) + (R2^ * v2)) = 0
* ...
* n^*v1 - n^*v2 + [n]x (p1 - p2) . w2 = 0
* -n^*v1 + n^*v2 + [n]x (p2 - p1) . w2 = 0
*
* Thus we see that
* J1l = -n
* J2l = n
* J1a = 0
* J2a = [n]x (p2 - p1)
* c = eps * ( (n^ * p1) - (n^ * a) )
*
*/
const int row0Index = 0;
const int row1Index = info->rowskip;
const int row2Index = 2 * row1Index;
const dReal eps = worldFPS * worldERP;
dVector3 globalAxis1;
dBodyVectorToWorld(node[0].body, axis1[0], axis1[1], axis1[2], globalAxis1);
dVector3 globalAxis2;
dVector3 globalAnchor;
if (node[1].body) {
dBodyVectorToWorld(node[1].body, axis2[0], axis2[1], axis2[2], globalAxis2);
dBodyGetRelPointPos(node[1].body, anchor2[0], anchor2[1], anchor2[2], globalAnchor);
} else {
dCopyVector3(globalAxis2, axis2);
dCopyVector3(globalAnchor, anchor2);
}
///////////////////////////////////////////////////////
// Angular velocity constraints (2 rows)
//
// Refer to the piston joint source code for details.
///////////////////////////////////////////////////////
// Find the 2 direction vectors of the plane.
dVector3 p, q;
dPlaneSpace ( globalAxis2, p, q );
// LHS 0
dCopyNegatedVector3 (info->J1a + row0Index, p );
dCopyNegatedVector3 (info->J1a + row1Index, q );
// LHS 1
dCopyVector3 (info->J2a + row0Index, p );
dCopyVector3 (info->J2a + row1Index, q );
// RHS 0 & 1 (absolute errors of the axis direction computed by dot product of the desired and actual axis)
dVector3 b;
dCalcVectorCross3( b, globalAxis2, globalAxis1 );
info->c[0] = eps * dCalcVectorDot3 ( p, b );
info->c[1] = eps * dCalcVectorDot3 ( q, b );
//////////////////////////////////////////////////////////////////////////////////////////
// Linear velocity constraint (1 row, removes 1 translational DOF along the plane normal)
//
// Only body1 should be restricted by the plane, which is moved along with body2.
//////////////////////////////////////////////////////////////////////////////////////////
dVector3 body1Center;
dCopyVector3(body1Center, node[0].body->posr.pos);
dVector3 body2Center = {0, 0, 0};
if (node[1].body) {
dCopyVector3(body2Center, node[1].body->posr.pos);
}
// LHS 2
dCopyNegatedVector3(info->J1l + row2Index, globalAxis2);
dCopyVector3( info->J2l + row2Index, globalAxis2);
dVector3 body2_body1;
dSubtractVectors3(body2_body1, body2Center, body1Center);
dCalcVectorCross3(info->J2a + row2Index, globalAxis2, body2_body1);
// RHS 2 (distance of body1 center from the plane)
info->c[2] = eps * (dCalcVectorDot3(globalAxis2, body1Center) - dCalcVectorDot3(globalAxis2, globalAnchor));
}
