private final void FOO2(int i, int j, int op, DContactGeomBuffer contacts, int skip, DVector3 p, DMatrix3C R, DVector3 side) { contacts.get(i*skip).pos.eqSum(p, R.viewCol(j), op*side.get(j)); }
private final void FOO2(int i, int j, int op, DContactGeomBuffer contacts, int skip, DVector3 p, DMatrix3C R, DVector3 side) { contacts.get(i*skip).pos.eqSum(p, R.viewCol(j), op*side.get(j)); }
private void dGeomRayGet (DVector3 start, DVector3 dir) { // dUASSERT (type == dRayClass,"argument not a ray"); recomputePosr(); // start[0] = g.final_posr.pos[0]; // start[1] = g.final_posr.pos[1]; // start[2] = g.final_posr.pos[2]; start.set(final_posr().pos()); // dir[0] = g.final_posr.R[0*4+2]; // dir[1] = g.final_posr.R[1*4+2]; // dir[2] = g.final_posr.R[2*4+2]; dir.set(final_posr().R().viewCol(2)); }
private void dGeomRayGet (DVector3 start, DVector3 dir) { // dUASSERT (type == dRayClass,"argument not a ray"); recomputePosr(); // start[0] = g.final_posr.pos[0]; // start[1] = g.final_posr.pos[1]; // start[2] = g.final_posr.pos[2]; start.set(final_posr().pos()); // dir[0] = g.final_posr.R[0*4+2]; // dir[1] = g.final_posr.R[1*4+2]; // dir[2] = g.final_posr.R[2*4+2]; dir.set(final_posr().R().viewCol(2)); }
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 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)
e.eqSum(final_posr().R().viewCol(2), _length, final_posr().pos(), 1);
e.eqSum(final_posr().R().viewCol(2), _length, final_posr().pos(), 1);
q.eqSum( ccyl.final_posr().R().viewCol(2), k, cs, -1); C = dCalcVectorDot3(q,q) - ccyl.getRadius()*ccyl.getRadius(); r.eqSum(ccyl.final_posr().R().viewCol(2), k, ccyl.final_posr().pos(), 1.0); if ((ray.final_posr().pos().get0()-r.get0())*(ray.final_posr().pos().get0()-r.get0()) + (ray.final_posr().pos().get1()-r.get1())*(ray.final_posr().pos().get1()-r.get1()) + r.eqSum(ccyl.final_posr().R().viewCol(2), uv, ray.final_posr().R().viewCol(2), -1); double A = dCalcVectorDot3(r,r); double B = 2*dCalcVectorDot3(q,r); contact.pos.eqSum( ray.final_posr().R().viewCol(2), alpha, ray.final_posr().pos(), 1); contact.normal.eqSum( ccyl.final_posr().R().viewCol(2), -k, ccyl.final_posr().pos(), -1); q.eqSum( ccyl.final_posr().R().viewCol(2), k, ccyl.final_posr().pos(), 1.0 ); return ray_sphere_helper (ray,q,ccyl.getRadius(),contacts, inside_ccyl);
q.eqSum( ccyl.final_posr().R().viewCol(2), k, cs, -1); C = dCalcVectorDot3(q,q) - ccyl.getRadius()*ccyl.getRadius(); r.eqSum(ccyl.final_posr().R().viewCol(2), k, ccyl.final_posr().pos(), 1.0); if ((ray.final_posr().pos().get0()-r.get0())*(ray.final_posr().pos().get0()-r.get0()) + (ray.final_posr().pos().get1()-r.get1())*(ray.final_posr().pos().get1()-r.get1()) + r.eqSum(ccyl.final_posr().R().viewCol(2), uv, ray.final_posr().R().viewCol(2), -1); double A = dCalcVectorDot3(r,r); double B = 2*dCalcVectorDot3(q,r); contact.pos.eqSum( ray.final_posr().R().viewCol(2), alpha, ray.final_posr().pos(), 1); contact.normal.eqSum( ccyl.final_posr().R().viewCol(2), -k, ccyl.final_posr().pos(), -1); q.eqSum( ccyl.final_posr().R().viewCol(2), k, ccyl.final_posr().pos(), 1.0 ); return ray_sphere_helper (ray,q,ccyl.getRadius(),contacts, inside_ccyl);
contact.pos.eqSum(ray.final_posr().pos(), ray.final_posr().R().viewCol(2), alpha); double nsign = (C < 0 || mode) ? (-1.0) : (1.0);
contact.pos.eqSum(ray.final_posr().pos(), ray.final_posr().R().viewCol(2), alpha); double nsign = (C < 0 || mode) ? (-1.0) : (1.0);
double sign = ( planePos.dot( o1.final_posr().R().viewCol(2) ) > 0) ? (-1.0) : (1.0);
double sign = ( planePos.dot( o1.final_posr().R().viewCol(2) ) > 0) ? (-1.0) : (1.0);