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(); }