void dJointSetDHingeAxis( DVector3C xyz ) { //dBodyVectorFromWorld(node[0].body, x, y, z, axis1); node[0].body.vectorFromWorld(xyz, axis1); if (node[1].body != null) { //dBodyVectorFromWorld(node[1].body, x, y, z, axis2); node[1].body.vectorFromWorld(xyz, axis2); } else { // joint->axis2[0] = x; // joint->axis2[1] = y; // joint->axis2[2] = z; axis2.set(xyz); } axis1.normalize(); axis1.normalize(); }
void dJointSetDHingeAxis( DVector3C xyz ) { //dBodyVectorFromWorld(node[0].body, x, y, z, axis1); node[0].body.vectorFromWorld(xyz, axis1); if (node[1].body != null) { //dBodyVectorFromWorld(node[1].body, x, y, z, axis2); node[1].body.vectorFromWorld(xyz, axis2); } else { // joint->axis2[0] = x; // joint->axis2[1] = y; // joint->axis2[2] = z; axis2.set(xyz); } axis1.normalize(); axis1.normalize(); }
void dJointSetTransmissionAxis2( DVector3C xyz ) { dUASSERT(mode == TRANSMISSION.dTransmissionIntersectingAxes, "can't set individual axes in current mode" ); if (node[1].body != null) { //dBodyVectorFromWorld(joint->node[1].body, x, y, z, joint->axes[1]); node[1].body.vectorFromWorld(xyz, axes[1]); axes[1].normalize(); } update = true; }
void dJointSetTransmissionAxis1( DVector3C xyz ) { dUASSERT(mode == TRANSMISSION.dTransmissionIntersectingAxes, "can't set individual axes in current mode" ); if (node[0].body != null) { //dBodyVectorFromWorld(joint->node[0].body, x, y, z, joint->axes[0]); node[0].body.vectorFromWorld(xyz, axes[0]); axes[0].normalize(); } update = true; }
void dJointSetTransmissionAxis2( DVector3C xyz ) { dUASSERT(mode == TRANSMISSION.dTransmissionIntersectingAxes, "can't set individual axes in current mode" ); if (node[1].body != null) { //dBodyVectorFromWorld(joint->node[1].body, x, y, z, joint->axes[1]); node[1].body.vectorFromWorld(xyz, axes[1]); axes[1].normalize(); } update = true; }
void dJointSetTransmissionAxis1( DVector3C xyz ) { dUASSERT(mode == TRANSMISSION.dTransmissionIntersectingAxes, "can't set individual axes in current mode" ); if (node[0].body != null) { //dBodyVectorFromWorld(joint->node[0].body, x, y, z, joint->axes[0]); node[0].body.vectorFromWorld(xyz, axes[0]); axes[0].normalize(); } update = true; }
void dJointSetTransmissionAxis( DVector3C xyz ) { int i; dUASSERT(mode == TRANSMISSION.dTransmissionParallelAxes || mode == TRANSMISSION.dTransmissionChainDrive , "axes must be set individualy in current mode" ); for (i = 0 ; i < 2 ; i += 1) { if (node[i].body != null) { //dBodyVectorFromWorld(joint->node[i].body, x, y, z, joint->axes[i]); node[i].body.vectorFromWorld(xyz, axes[i]); axes[i].normalize(); } } update = true; }
void dJointSetTransmissionAxis( DVector3C xyz ) { int i; dUASSERT(mode == TRANSMISSION.dTransmissionParallelAxes || mode == TRANSMISSION.dTransmissionChainDrive , "axes must be set individualy in current mode" ); for (i = 0 ; i < 2 ; i += 1) { if (node[i].body != null) { //dBodyVectorFromWorld(joint->node[i].body, x, y, z, joint->axes[i]); node[i].body.vectorFromWorld(xyz, axes[i]); axes[i].normalize(); } } update = true; }
private void testPlaneSpace() { HEADER(); DVector3 n = new DVector3(),p = new DVector3(),q = new DVector3(); int bad = 0; for (int i=0; i<1000; i++) { dMakeRandomVector (n,1.0); n.normalize(); dPlaneSpace (n,p,q); if (Math.abs(n.dot(p)) > tol) bad = 1; if (Math.abs(n.dot(q)) > tol) bad = 1; if (Math.abs(p.dot(q)) > tol) bad = 1; if (Math.abs(p.dot(p)-1) > tol) bad = 1; if (Math.abs(q.dot(q)-1) > tol) bad = 1; } println ("\t", bad != 0 ? "FAILED" : "passed"); }
private void dGeomRaySet (DVector3C p, DVector3C d) { recomputePosr(); DMatrix3C rot = final_posr().R(); _final_posr.pos.set(p); DVector3 n = new DVector3(d); n.normalize(); rot.viewCol(2).set(n); dGeomMoved (); }
private void dGeomRaySet (DVector3C p, DVector3C d) { recomputePosr(); DMatrix3C rot = final_posr().R(); _final_posr.pos.set(p); DVector3 n = new DVector3(d); n.normalize(); rot.viewCol(2).set(n); dGeomMoved (); }
private void testNormalize3() { HEADER(); int i,bad=0; DVector3 n1 = new DVector3(),n2 = new DVector3(); for (i=0; i<1000; i++) { dMakeRandomVector (n1,1.0); n2.set(n1); n2.normalize(); if (Math.abs(n2.dot(n2) - 1.0) > tol) bad |= 1; if (Math.abs(n2.get0()/n1.get0() - n2.get1()/n1.get1()) > tol) bad |= 2; if (Math.abs(n2.get0()/n1.get0() - n2.get2()/n1.get2()) > tol) bad |= 4; if (Math.abs(n2.get1()/n1.get1() - n2.get2()/n1.get2()) > tol) bad |= 8; if (Math.abs(n2.dot(n1) - Math.sqrt(n1.dot(n1))) > tol) bad |= 16; if (bad != 0) { println ("\tFAILED (code=" + bad + ")"); return; } } println ("\tpassed"); }
private void dGeomRaySet (double px, double py, double pz, double dx, double dy, double dz) { // dUASSERT (g!=null && g.type == dRayClass,"argument not a ray"); recomputePosr(); DMatrix3C rot = final_posr().R(); // dVector3 pos = final_posr().pos(); DVector3 n; // pos[0] = px; // pos[1] = py; // pos[2] = pz; _final_posr.pos.set(px, py, pz); // n[0] = dx; // n[1] = dy; // n[2] = dz; n = new DVector3(dx, dy, dz); //dNormalize3(n); n.normalize(); // rot[0*4+2] = n[0]; // rot[1*4+2] = n[1]; // rot[2*4+2] = n[2]; rot.viewCol(2).set(n); dGeomMoved (); } private void dGeomRaySet (DVector3C p, DVector3C d)
private void dGeomRaySet (double px, double py, double pz, double dx, double dy, double dz) { // dUASSERT (g!=null && g.type == dRayClass,"argument not a ray"); recomputePosr(); DMatrix3C rot = final_posr().R(); // dVector3 pos = final_posr().pos(); DVector3 n; // pos[0] = px; // pos[1] = py; // pos[2] = pz; _final_posr.pos.set(px, py, pz); // n[0] = dx; // n[1] = dy; // n[2] = dz; n = new DVector3(dx, dy, dz); //dNormalize3(n); n.normalize(); // rot[0*4+2] = n[0]; // rot[1*4+2] = n[1]; // rot[2*4+2] = n[2]; rot.viewCol(2).set(n); dGeomMoved (); } private void dGeomRaySet (DVector3C p, DVector3C d)
private void makeRandomRotation (DMatrix3 R) { //double *u1 = R, *u2=R+4, *u3=R+8; DVector3 u1 = new DVector3(R.get00(), R.get01(), R.get02()); //TZ DVector3 u2 = new DVector3(R.get10(), R.get11(), R.get12()); //TZ DVector3 u3 = new DVector3(R.get20(), R.get21(), R.get22()); //TZ //dMakeRandomVector (u1P,3,1.0); dMakeRandomVector (u1, 1.0); u1.normalize(); dMakeRandomVector (u2, 1.0); double d = u1.dot (u2); // u2[0] -= d*u1[0]; // u2[1] -= d*u1[1]; // u2[2] -= d*u1[2]; u2.eqSum( u2, u1, -d ); u2.normalize(); u3.eqCross(u1,u2); //TZ back to R R.setCol(0, u1); R.setCol(1, u2); R.setCol(2, u3); }
public static void dRFromZAxis (DMatrix3 R, DVector3C xyz) { DVector3 n = new DVector3(),p = new DVector3(),q = new DVector3(); n.set(xyz); n.normalize(); OdeMath.dPlaneSpace (n,p,q); R.set( p.get0(), q.get0(), n.get0(), p.get1(), q.get1(), n.get1(), p.get2(), q.get2(), n.get2()); }
public static void dRFromZAxis (DMatrix3 R, DVector3C xyz) { DVector3 n = new DVector3(),p = new DVector3(),q = new DVector3(); n.set(xyz); n.normalize(); OdeMath.dPlaneSpace (n,p,q); R.set( p.get0(), q.get0(), n.get0(), p.get1(), q.get1(), n.get1(), p.get2(), q.get2(), n.get2()); }
n.normalize(); d = dRandReal() - 0.5; plane.setParams (n,d);