void Balaenidae::doDynamics(dBodyID body)
{
Vec3f Ft;
Ft[0]=0;Ft[1]=0;Ft[2]=getThrottle();
dBodyAddRelForce(body,Ft[0],Ft[1],Ft[2]);
dBodyAddRelTorque(body,0.0f,Balaenidae::rudder*1000,0.0f);
if (offshoring == 1) {
offshoring=0;
setStatus(Balaenidae::SAILING);
}
else if (offshoring > 0)
{
// Add a retractive force to keep it out of the island.
Vec3f ap = Balaenidae::ap;
setThrottle(0.0);
Vec3f V = ap*(-10000);
dBodyAddRelForce(body,V[0],V[1],V[2]);
offshoring--;
}
// Buyoncy
//if (pos[1]<0.0f)
// dBodyAddRelForce(me,0.0,9.81*20050.0f,0.0);
dReal *v = (dReal *)dBodyGetLinearVel(body);
dVector3 O;
dBodyGetRelPointPos( body, 0,0,0, O);
dVector3 F;
dBodyGetRelPointPos( body, 0,0,1, F);
F[0] = (F[0]-O[0]);
F[1] = (F[1]-O[1]);
F[2] = (F[2]-O[2]);
Vec3f vec3fF;
vec3fF[0] = F[0];vec3fF[1] = F[1]; vec3fF[2] = F[2];
Vec3f vec3fV;
vec3fV[0]= v[0];vec3fV[1] = v[1]; vec3fV[2] = v[2];
speed = vec3fV.magnitude();
VERIFY(speed, me);
vec3fV = vec3fV * 0.02f;
dBodyAddRelForce(body,vec3fV[0],vec3fV[1],vec3fV[2]);
wrapDynamics(body);
}
void
dxJointDBall::updateTargetDistance()
{
dVector3 p1, p2;
if (node[0].body)
dBodyGetRelPointPos(node[0].body, anchor1[0], anchor1[1], anchor1[2], p1);
else
dCopyVector3(p1, anchor1);
if (node[1].body)
dBodyGetRelPointPos(node[1].body, anchor2[0], anchor2[1], anchor2[2], p2);
else
dCopyVector3(p2, anchor2);
targetDistance = dCalcPointsDistance3(p1, p2);
}
/**
Grab the data from the model and stash it
here.
*/
void MarkerData::captureVirtualMarkers()
{
dVector4 pt;
C3dFloatFrame* frame = new C3dFloatFrame(marker_count);
C3dFloatFrame* shadowFrame = new C3dFloatFrame(marker_count);
for (int ii=0;ii<marker_count;++ii) {
if (body_pointer->marker_to_body[ii].id>=0) {
// Find the global coordinate of the
// relative position to which the
// marker is mapped
dBodyGetRelPointPos(body_pointer->body_segments[body_pointer->marker_to_body[ii].id],
body_pointer->marker_to_body[ii].position[0],
body_pointer->marker_to_body[ii].position[1],
body_pointer->marker_to_body[ii].position[2],
pt);
pt[3]=1;
} else {
pt[0]=pt[1]=pt[2]=0;
pt[3]=-1;
}
frame->data[ii].point[0]=pt[0]*1000;
frame->data[ii].point[1]=pt[1]*1000;
frame->data[ii].point[2]=pt[2]*1000;
frame->data[ii].point[3]=pt[3];
}
virtual_data.push_back(frame);
memcpy(shadowFrame->data,frame->data,marker_count*sizeof(FPoint));
shadow_data.push_back(shadowFrame);
}
void odCable::run(){
int i;
double pi=4.*atan(1.);
double volume, segLen, rho, x, u, w, alpha, uwMag, alphaMap;
odPoint P0, F, V, axial, radial;
dVector3 P;
// The opendynamics solver handles connections by default (unless we wish to do something odd with them).
// dBodyAddRelForceAtRelPos(element[nSegments-1], 50, 0, 50, 0, 0, 0);
// Axial and radial directions in local element co-ordinates.
axial=odPoint(1,0,0);
radial=odPoint(0,0,1);
segLen=length/(double)nSegments;
volume=pi*pow((diameter/2),2)*segLen;
for (i=0;i<nSegments;i++){
// Temporarily make the current element the active body...
odeBody=&element[i];
// Add buoyant force...
dBodyGetRelPointPos(element[i], 0, 0, 0, P);
P0=odPoint(P[0],P[1],P[2]);
rho=density(P0);
V=localVelocity(P0, uid);
// printf("Centre Location = %lf\t%lf\t%lf\t\n", P[0], P[1], P[2]);
F=odPoint(0,0,1)*constants->g*volume*rho;
// printf("Volume = %lf\n", volume);
// printf("Density= %lf\n", density(odPoint(P[0],P[1],P[2])));
// printf("Buoyant Force = %lf\t%lf\t%lf\n", F.x, F.y, F.z);
// dBodyAddForceAtRelPos(element[nSegments-1], F.x, F.y, F.z, 0, 0, 0);
if (i==0){
// Add resistance force...
V.x=1;
V.y=0;
V.z=0;
printf("Flow Speed = %lf\t%lf\t%lf\n", V.x, V.y, V.z);
u=radial.dotProduct_natural(-V);
w=axial.dotProduct_natural(-V);
uwMag=sqrt(u*u+w*w);
printf("u=%lf\tw=%lf\tuwMAG=%lf\n",u,w,uwMag);
alpha=atan2(w,u);
printf("Alpha %lf\n",alpha*180./pi);
// map to 0-90 range for convenience...
alphaMap=fabs(alpha*180./pi);
if (alphaMap>90) alphaMap=90-alphaMap;
printf("AlphaMap %lf\n",alphaMap);
// x=0.5*rho*V.mag()*segLen*diameter*0.5;
// if (*simTime<20.) x*=*simTime/20.;
// F=-V/V.mag()*x; // Multiply by the inverse of the velocity vector (if the body is moving with v- then drag is in the direction of v+
printf("Drag Force = %lf\t%lf\t%lf\n", F.x, F.y, F.z);
// dBodyAddForceAtRelPos(element[nSegments-1], F.x, F.y, F.z, 0, 0, 0);
}
}
}
void dJointGetDBallAnchor2( dJointID j, dVector3 result )
{
dxJointDBall* joint = dynamic_cast<dxJointDBall*>(j);
dUASSERT( joint, "bad joint argument" );
dUASSERT( result, "bad result argument" );
if ( joint->flags & dJOINT_REVERSE ) {
if (joint->node[0].body)
dBodyGetRelPointPos(joint->node[0].body, joint->anchor1[0], joint->anchor1[1], joint->anchor1[2], result);
else
dCopyVector3(result, joint->anchor1);
} else {
if (joint->node[1].body)
dBodyGetRelPointPos(joint->node[1].body, joint->anchor2[0], joint->anchor2[1], joint->anchor2[2], result);
else
dCopyVector3(result, joint->anchor2);
}
}
void ODE_Link::getTransform(hrp::Vector3& pos_, hrp::Matrix33& R_){
const dReal* R = dBodyGetRotation(bodyId);
R_ << R[0],R[1],R[2],
R[4],R[5],R[6],
R[8],R[9],R[10];
dVector3 result;
dBodyGetRelPointPos(bodyId, -C[0], -C[1], -C[2], result);
pos_ << result[0], result[1], result[2];
}
void dJointGetTransmissionAnchor2( 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) {
dBodyGetRelPointPos(joint->node[1].body,
joint->anchors[1][0],
joint->anchors[1][1],
joint->anchors[1][2],
result);
}
}
/*!
* Transform this polygon and return the output
* as new polygon. In this transform we apply a
* rigid transform, so whilst the following will
* change:
* - Projected area
* - The centroid
* - The normal vector
* The following won't:
* - Surface area
*/
odPolygon odPolygon::transformed(dBodyID *odeBody){
int i;
odPolygon outPoly;
dVector3 result;
for (i=0;i<pt.size();i++){
dBodyGetRelPointPos(*odeBody, pt[i].x, pt[i].y, pt[i].z, result);
outPoly.pt.push_back(odPoint(result[0], result[1], result[2]));
}
outPoly.srfArea=srfArea;
outPoly.projectedAreaXY();
outPoly.centroid();
outPoly.normal();
return outPoly;
}
/*!
* For plotting purposes all sections must have the same number of points.
*/
void odMesh::plot(){
int nPoints, i, j, triPt;
dVector3 result;
points = vtkPoints::New();
triPt=0;
for (i=0;i<(int)polygon.size();i++){
triangle.push_back( vtkTriangle::New() );
for (j=0;j<(int)polygon[i].pt.size();j++){
dBodyGetRelPointPos(*odeBody, polygon[i].pt[j].x, polygon[i].pt[j].y, polygon[i].pt[j].z, result);
points->InsertNextPoint(result[0], result[1], result[2]);
triangle.back()->GetPointIds()->SetId ( j, triPt );
triPt++;
}
objects->InsertNextCell(triangle.back());
}
// Add the geometry and topology to the polydata
polygons->SetPoints (points);
polygons->SetPolys (objects);
actor->GetProperty()->SetColor( 1.0, 1.0, 0 );
}
void ODE_Particle::GetRelPointPos(dReal px, dReal py, dReal pz,dVector3 &result)
{
dBodyGetRelPointPos(body,px,py,pz,result);
}
void odCable::exportVTK(FILE *stream){
int i;
double segLen=length/(double)nSegments;
dVector3 P, j0, j1;
odPoint d;
fprintf(stream,"<?xml version=\"1.0\"?>\n");
fprintf(stream,"<VTKFile type=\"PolyData\" version=\"0.1\" byte_order=\"LittleEndian\">\n");
fprintf(stream,"<PolyData>\n");
fprintf(stream," <Piece NumberOfPoints=\"%i\" NumberOfLines=\"%i\" >\n", nSegments*2, nSegments);
fprintf(stream," <Points>\n");
fprintf(stream," <DataArray type=\"Float32\" NumberOfComponents=\"3\" format=\"ascii\">\n");
for (i=0;i<nSegments;i++){
dBodyGetRelPointPos (element[i], -segLen/2., 0, 0, P);
fprintf(stream,"%lf %lf %lf ", P[0], P[1], P[2]);
dBodyGetRelPointPos (element[i], segLen/2., 0, 0, P);
fprintf(stream,"%lf %lf %lf\n", P[0], P[1], P[2]);
}
fprintf(stream," </DataArray>\n");
fprintf(stream," </Points>\n");
fprintf(stream," <Lines>\n");
fprintf(stream," <DataArray type=\"Int32\" Name=\"connectivity\" format=\"ascii\">\n");
for (i=0;i<nSegments;i++){
fprintf(stream,"%i %i\n",2*i, 2*i+1);
}
fprintf(stream," </DataArray>\n");
fprintf(stream," <DataArray type=\"Int32\" Name=\"offsets\" format=\"ascii\">\n");
for (i=0;i<nSegments;i++){
fprintf(stream,"%i\n", 2*i+2);
}
fprintf(stream," </DataArray>\n");
fprintf(stream," </Lines>\n");
/*
fprintf(stream," <PointData>\n");
fprintf(stream," <DataArray type=\"Float32\" Name=\"Force\" NumberOfComponents=\"3\" format=\"ascii\">\n");
fprintf(stream,"%lf %lf %lf ", force1.x, force1.y, force1.z);
fprintf(stream,"%lf %lf %lf\n", force2.x, force2.y, force2.z);
fprintf(stream," </DataArray>\n");
fprintf(stream," </PointData>\n");
*/
fprintf(stream," <CellData>\n");
fprintf(stream," <DataArray type=\"Float32\" Name=\"Element Length\" NumberOfComponents=\"1\" format=\"ascii\">\n");
// First length from end1 to joint 0
dJointGetBallAnchor(end1, j0);
dJointGetBallAnchor(joint[0], j1);
d=odPoint(j0[0], j0[1], j0[2])-odPoint(j1[0], j1[1], j1[2]);
fprintf(stream,"%lf\n", d.mag());
for (i=0;i<nSegments-2;i++){
dJointGetBallAnchor(joint[i], j0);
dJointGetBallAnchor(joint[i+1], j1);
d=odPoint(j0[0], j0[1], j0[2])-odPoint(j1[0], j1[1], j1[2]);
fprintf(stream,"%lf\n", d.mag());
}
fprintf(stream,"%lf\n", 0);
fprintf(stream," </DataArray>\n");
fprintf(stream," </CellData>\n");
fprintf(stream," <CellData>\n");
fprintf(stream," <DataArray type=\"Int32\" Name=\"Element ID\" NumberOfComponents=\"1\" format=\"ascii\">\n");
for (i=0;i<nSegments;i++){
fprintf(stream,"%i\n", i);
}
fprintf(stream,"%lf\n", 0);
fprintf(stream," </DataArray>\n");
fprintf(stream," </CellData>\n");
fprintf(stream," </Piece>\n");
fprintf(stream,"</PolyData>\n");
fprintf(stream,"</VTKFile>\n");
}
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));
}
/* returns the position of a point according to the local frame: r = x u + y v + z t */
void body_point_position (t_real *res, dBodyID b, t_real x, t_real y, t_real z) {
dBodyGetRelPointPos (b, x, y, z, res);
}
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 PhysicsBody::getRelPointPos(const Vec3f &v, Vec3f &result)
{
dVector3 t;
dBodyGetRelPointPos(_BodyID, v.x(), v.y(), v.z(), t);
result.setValue(Vec3f(t[0], t[1], t[2]));
}
void Machine::fire(void)
{
if(powerupammo>0 && powerupcharge==0)
{
if(poweruptype==0)
{
powerupammo--;
turbocount=3;
powerupcount=1;
play_sample(&mb_turbo_wav, 255, 128, 1000, 0);
}
if(poweruptype==2)
{
dVector3 result;
dBodyGetRelPointPos(body[0], 0, -9, -5, result);
if(result[2]<5)
{
powerupammo--;
powerupcharge=0.25;
if(this==&machine[0])
mines.addMine(result[0], result[1], 0);
else
mines.addMine(result[0], result[1], 1);
play_sample(&mb_minelay_wav, 255, 128, 1000, 0);
}
}
if(poweruptype==3 || poweruptype==4)
{
const dReal * pos0 = dBodyGetPosition(body[0]);
const dReal * pos1 = dBodyGetPosition(body[1]);
double length=sqrt(
(pos0[0]-pos1[0])*(pos0[0]-pos1[0]) +
(pos0[1]-pos1[1])*(pos0[1]-pos1[1]) +
(pos0[2]-pos1[2])*(pos0[2]-pos1[2]))+3;
dVector3 result;
dBodyGetRelPointPos(body[0], 0, length+5, 0, result);
if(result[2]<10)
{
powerupammo--;
powerupcharge=0.5;
const dReal *p = dBodyGetRotation(body[0]);
double x=p[1], y=p[5];
length = sqrt(x*x+y*y);
x/=length;
y/=length;
double angle=-acos(x)*(180/M_PI);
if(y<0)
angle=-angle;
angle+=90;
if(poweruptype==3)
{
if(this==&machine[0])
projectiles.addProjectile(result[0], result[1], x*130, y*100, angle, 0, 0);
else
projectiles.addProjectile(result[0], result[1], x*130, y*100, angle, 0, 1);
play_sample(&mb_mlaunch1_wav, 255, 128, 1000, 0);
}
else
{
if(this==&machine[0])
projectiles.addProjectile(result[0], result[1], x*160, y*160, angle, 1, 0);
else
projectiles.addProjectile(result[0], result[1], x*160, y*160, angle, 1, 1);
play_sample(&mb_mlaunch2_wav, 255, 128, 1000, 0);
}
}
}
}
}
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 Simulation::simLoop(int pause)
{
static dReal last_capture_time = 0.0;
static dReal capture_dt = 1.0 / 100; // for 100 fps in simulation time
// profiling variables
this->callback_count = this->not_ground_count = this->ground_count = 0;
// use timer to track time (you need to stop the watch to update the time.
// apparently the digits change so fast it can't be read :-)
//dStopwatchStop(&this->timer);
//dStopwatchStart(&this->timer);
// if there is an automat running, run it. if it is done, delete and *zero* it
this->mainMovie();
// capture a picture every capture_dt second
#ifdef ODE_FRAME_PATCH
clearWriteFrames();
#endif
//cout << "debug: " << (dStopwatchTime(&timer)) << "\n";
if (mywriteframes && (simTime - last_capture_time > capture_dt)) {
#ifdef ODE_FRAME_PATCH
setWriteFrames();
#endif
PEXP(this->simTime);
last_capture_time = this->simTime;
}
// apply forces
for (int i = 0; i < 2; ++i) {
if (twitch_flag) {
this->car->setSpeed(i,
(int) ((dStopwatchTime(&timer) *
twitch_hertz)) %
2 ? min_speed : max_speed);
} else {
this->car->setSpeed(i, s_speed[i]);
}
}
// Accounting
// - zero number of contacts in each chain
this->car->chain_obj[0]->ZeroUserData();
this->car->chain_obj[1]->ZeroUserData();
// oh, if this was ruby!
// car.chains.each { |chain| chain.ZeroUserData() }
// time step
if (!pause) {
dSpaceCollide(space, 0, &::nearCallback);
//if (contactgroup->num) PEXP(contactgroup->num);
// Test fast step
if (use_step_fast)
//dWorldStepFast1(world, step, iterations);
dWorldQuickStep(world, step);
else
dWorldStep(world, step);
simTime += step;
// remove all contact joints
dJointGroupEmpty(contactgroup);
}
// profiling
//printf("debug: callback %4d not_ground %4d ground %4d\n", callback_count,
// not_ground_count, ground_count);
// draw
//dsSetTexture (DS_WOOD);
// set camera if it is tracking the body
if (lockCamera) {
float xyz[3], hpr[3];
dVector3 newpoint;
dsGetViewpoint(xyz, hpr);
dBodyGetRelPointPos(camera_object, cameraRelVec[0],
cameraRelVec[1], cameraRelVec[2], newpoint);
xyz[0] = newpoint[0]; xyz[1] = newpoint[1]; xyz[2] = newpoint[2];
dsSetViewpoint(xyz, hpr);
}
if (objbin.first)
objbin.first->DrawChain();
}
