void dBodyGetPointVel (DVector3C prel, DVector3 result) { DVector3 p = new DVector3(prel).sub(_posr.pos()); result.set(lvel); dAddVectorCross3 (result,avel,p); }
void dBodyGetPointVel (DVector3C prel, DVector3 result) { DVector3 p = new DVector3(prel).sub(_posr.pos()); result.set(lvel); dAddVectorCross3 (result,avel,p); }
public double dGeomSpherePointDepth (double x, double y, double z) { //dUASSERT (g && g.type == dSphereClass,"argument not a sphere"); recomputePosr(); //dxSphere s = (dxSphere) g; // double [] pos = final_posr.pos.v; // return _radius - dSqrt ((x-pos[0])*(x-pos[0]) + // (y-pos[1])*(y-pos[1]) + // (z-pos[2])*(z-pos[2])); DVector3 pos = new DVector3(x, y, z); pos.sub(final_posr().pos()); return _radius - pos.length(); } public double dGeomSpherePointDepth (DVector3C xyz)
public double dGeomSpherePointDepth (double x, double y, double z) { //dUASSERT (g && g.type == dSphereClass,"argument not a sphere"); recomputePosr(); //dxSphere s = (dxSphere) g; // double [] pos = final_posr.pos.v; // return _radius - dSqrt ((x-pos[0])*(x-pos[0]) + // (y-pos[1])*(y-pos[1]) + // (z-pos[2])*(z-pos[2])); DVector3 pos = new DVector3(x, y, z); pos.sub(final_posr().pos()); return _radius - pos.length(); } public double dGeomSpherePointDepth (DVector3C xyz)
void dGeomGetPosRelPoint(double px, double py, double pz, DVector3 result) { if ((_gflags & GEOM_PLACEABLE) == 0) { result.set(px, py, pz); return; } recomputePosr(); DVector3 prel = new DVector3(px, py, pz); prel.sub(_final_posr.pos()); // prel[0] = px - g->final_posr->pos[0]; // prel[1] = py - g->final_posr->pos[1]; // prel[2] = pz - g->final_posr->pos[2]; // prel[3] = 0; dMultiply1_331 (result,_final_posr.R(),prel); }
node[0].body._posr.pos.sub(dx, dy, dz);
node[0].body._posr.pos.sub(dx, dy, dz);
private float getAngle(int triangle, int vertexIndex, int neighbourTriangle) { DVector3 startVertex = getVertex(data.getIndexRef()[triangle * 3 + vertexIndex]); DVector3 endVertex = getVertex(data.getIndexRef()[triangle * 3 + (vertexIndex + 1) % 3]); DVector3 oppositeVertex = getVertex(data.getIndexRef()[triangle * 3 + (vertexIndex + 2) % 3]); DVector3 edgeAxis = new DVector3(endVertex).sub(startVertex); DVector3 secondEdge = new DVector3(oppositeVertex).sub(endVertex); DVector3 normal = new DVector3(); normal.eqCross(edgeAxis, secondEdge); DVector3 tangent = new DVector3(); tangent.eqCross(edgeAxis, normal); DVector3 neighbourVertex1 = getVertex(data.getIndexRef()[neighbourTriangle * 3]); DVector3 neighbourVertex2 = getVertex(data.getIndexRef()[neighbourTriangle * 3 + 1]); DVector3 neighbourVertex3 = getVertex(data.getIndexRef()[neighbourTriangle * 3 + 2]); DVector3 neighbourEdge1 = new DVector3(neighbourVertex2).sub(neighbourVertex1); DVector3 neighbourEdge2 = new DVector3(neighbourVertex3).sub(neighbourVertex2); DVector3 neighbourNormal = new DVector3(); neighbourNormal.eqCross(neighbourEdge1, neighbourEdge2); float angle = (float) Math.signum(tangent.dot(neighbourNormal)); double angleCos = normal.dot(neighbourNormal); double length = Math.sqrt(normal.lengthSquared() * neighbourNormal.lengthSquared()); if (length > Double.MIN_VALUE) { angleCos /= length; } angleCos = Math.max(Math.min(1.0, angleCos), -1.0); angle *= (float) Math.acos(angleCos); return angle; }
private float getAngle(int triangle, int vertexIndex, int neighbourTriangle) { DVector3 startVertex = getVertex(data.getIndexRef()[triangle * 3 + vertexIndex]); DVector3 endVertex = getVertex(data.getIndexRef()[triangle * 3 + (vertexIndex + 1) % 3]); DVector3 oppositeVertex = getVertex(data.getIndexRef()[triangle * 3 + (vertexIndex + 2) % 3]); DVector3 edgeAxis = new DVector3(endVertex).sub(startVertex); DVector3 secondEdge = new DVector3(oppositeVertex).sub(endVertex); DVector3 normal = new DVector3(); normal.eqCross(edgeAxis, secondEdge); DVector3 tangent = new DVector3(); tangent.eqCross(edgeAxis, normal); DVector3 neighbourVertex1 = getVertex(data.getIndexRef()[neighbourTriangle * 3]); DVector3 neighbourVertex2 = getVertex(data.getIndexRef()[neighbourTriangle * 3 + 1]); DVector3 neighbourVertex3 = getVertex(data.getIndexRef()[neighbourTriangle * 3 + 2]); DVector3 neighbourEdge1 = new DVector3(neighbourVertex2).sub(neighbourVertex1); DVector3 neighbourEdge2 = new DVector3(neighbourVertex3).sub(neighbourVertex2); DVector3 neighbourNormal = new DVector3(); neighbourNormal.eqCross(neighbourEdge1, neighbourEdge2); float angle = (float) Math.signum(tangent.dot(neighbourNormal)); double angleCos = normal.dot(neighbourNormal); double length = Math.sqrt(normal.lengthSquared() * neighbourNormal.lengthSquared()); if (length > Double.MIN_VALUE) { angleCos /= length; } angleCos = Math.max(Math.min(1.0, angleCos), -1.0); angle *= (float) Math.acos(angleCos); return angle; }
void dGeomGetPosRelPoint(double px, double py, double pz, DVector3 result) { if ((_gflags & GEOM_PLACEABLE) == 0) { result.set(px, py, pz); return; } recomputePosr(); DVector3 prel = new DVector3(px, py, pz); prel.sub(_final_posr.pos()); // prel[0] = px - g->final_posr->pos[0]; // prel[1] = py - g->final_posr->pos[1]; // prel[2] = pz - g->final_posr->pos[2]; // prel[3] = 0; dMultiply1_331 (result,_final_posr.R(),prel); }
void dJointSetPistonAxisDelta ( double x, double y, double z, double dx, double dy, double dz ) { setAxes ( x, y, z, axis1, axis2 ); computeInitialRelativeRotation(); DVector3 c = new DVector3(0,0,0);//= {0,0,0}; if ( node[1].body!=null ) { // c.v[0] = ( node[0].body._posr.pos.v[0] - // node[1].body._posr.pos.v[0] - dx ); // c.v[1] = ( node[0].body._posr.pos.v[1] - // node[1].body._posr.pos.v[1] - dy ); // c.v[2] = ( node[0].body._posr.pos.v[2] - // node[1].body._posr.pos.v[2] - dz ); c.eqDiff(node[0].body.posr().pos(), node[1].body.posr().pos()).sub(dx, dy, dz); } else if ( node[0].body!=null ) { // c.v[0] = node[0].body._posr.pos.v[0] - dx; // c.v[1] = node[0].body._posr.pos.v[1] - dy; // c.v[2] = node[0].body._posr.pos.v[2] - dz; c.set(node[0].body.posr().pos()).sub(dx, dy, dz); } // Convert into frame of body 1 dMultiply1_331 ( anchor1, node[0].body.posr().R(), c ); }
void dJointSetPistonAxisDelta ( double x, double y, double z, double dx, double dy, double dz ) { setAxes ( x, y, z, axis1, axis2 ); computeInitialRelativeRotation(); DVector3 c = new DVector3(0,0,0);//= {0,0,0}; if ( node[1].body!=null ) { // c.v[0] = ( node[0].body._posr.pos.v[0] - // node[1].body._posr.pos.v[0] - dx ); // c.v[1] = ( node[0].body._posr.pos.v[1] - // node[1].body._posr.pos.v[1] - dy ); // c.v[2] = ( node[0].body._posr.pos.v[2] - // node[1].body._posr.pos.v[2] - dz ); c.eqDiff(node[0].body.posr().pos(), node[1].body.posr().pos()).sub(dx, dy, dz); } else if ( node[0].body!=null ) { // c.v[0] = node[0].body._posr.pos.v[0] - dx; // c.v[1] = node[0].body._posr.pos.v[1] - dy; // c.v[2] = node[0].body._posr.pos.v[2] - dz; c.set(node[0].body.posr().pos()).sub(dx, dy, dz); } // Convert into frame of body 1 dMultiply1_331 ( anchor1, node[0].body.posr().R(), c ); }
private static boolean box1inside2 (final DVector3 p1, final DMatrix3 R1, final DVector3 side1, final DVector3 p2, final DMatrix3 R2, final DVector3 side2) { for (int i=-1; i<=1; i+=2) { for (int j=-1; j<=1; j+=2) { for (int k=-1; k<=1; k+=2) { DVector3 v=new DVector3(),vv=new DVector3(); v.set( side1 ).scale( i*0.5, j*0.5, k*0.5 ); dMultiply0_331 (vv,R1,v); vv.add(p1).sub(p2); for (int axis=0; axis < 3; axis++) { double z = dCalcVectorDot3_14(vv,R2,axis); if (z < (-side2.get(axis)*0.5) || z > (side2.get(axis)*0.5)) return false; } } } } return true; }
c3.pos.set( contacts.get(1*skip).pos ).add( contacts.get(2*skip).pos).sub(p); c3.depth = d4; ret.inc();
c3.pos.set( contacts.get(1*skip).pos ).add( contacts.get(2*skip).pos).sub(p); c3.depth = d4; ret.inc();
q.set( x, y, z ).sub( joint.node[1].body.posr().pos() );
public double dGeomCapsulePointDepth (double x, double y, double z) { // dUASSERT (g && g.type == dCapsuleClass,"argument not a ccylinder"); recomputePosr(); // dxCapsule *c = (dxCapsule*) g; final DMatrix3C R = final_posr().R(); final DVector3C pos = final_posr().pos(); DVector3 a = new DVector3(x, y, z); // a[0] = x - pos[0]; // a[1] = y - pos[1]; // a[2] = z - pos[2]; a.sub(pos); double beta = dCalcVectorDot3_14(a,R,2); double lz2 = _lz*(0.5); if (beta < -lz2) beta = -lz2; else if (beta > lz2) beta = lz2; // a.v[0] = _final_posr.pos.v[0] + beta*R.v[0*4+2]; // a.v[1] = _final_posr.pos.v[1] + beta*R.v[1*4+2]; // a.v[2] = _final_posr.pos.v[2] + beta*R.v[2*4+2]; a.eqSum(final_posr().pos(), R.columnAsNewVector(2), beta); return _radius - dSqrt ((x-a.get0())*(x-a.get0()) + (y-a.get1())*(y-a.get1()) + (z-a.get2())*(z-a.get2())); }
public double dGeomCapsulePointDepth (double x, double y, double z) { // dUASSERT (g && g.type == dCapsuleClass,"argument not a ccylinder"); recomputePosr(); // dxCapsule *c = (dxCapsule*) g; final DMatrix3C R = final_posr().R(); final DVector3C pos = final_posr().pos(); DVector3 a = new DVector3(x, y, z); // a[0] = x - pos[0]; // a[1] = y - pos[1]; // a[2] = z - pos[2]; a.sub(pos); double beta = dCalcVectorDot3_14(a,R,2); double lz2 = _lz*(0.5); if (beta < -lz2) beta = -lz2; else if (beta > lz2) beta = lz2; // a.v[0] = _final_posr.pos.v[0] + beta*R.v[0*4+2]; // a.v[1] = _final_posr.pos.v[1] + beta*R.v[1*4+2]; // a.v[2] = _final_posr.pos.v[2] + beta*R.v[2*4+2]; a.eqSum(final_posr().pos(), R.columnAsNewVector(2), beta); return _radius - dSqrt ((x-a.get0())*(x-a.get0()) + (y-a.get1())*(y-a.get1()) + (z-a.get2())*(z-a.get2())); }
q.sub(node[1].body.posr().pos());
geom.setBody(body); DVector3 za = new DVector3(p2); (za.sub(p1)).safeNormalize(); DVector3 xa; DVector3 ya;