/** * * @return The first contact. */ public DContactGeom get() { return get(0); }
/** * * @return The first contact. */ public DContactGeom get() { return get(0); }
public DContactGeom getSafe(int flags, int index) { if (!(index >= 0 && index < (flags & DxGeom.NUMC_MASK))) { throw new IllegalStateException("Index="+index + "; flags="+flags); } return get(index); }
public DContactGeom getSafe(int flags, int index) { if (!(index >= 0 && index < (flags & DxGeom.NUMC_MASK))) { throw new IllegalStateException("Index="+index + "; flags="+flags); } return get(index); }
double d4 = contacts.get(1*skip).depth + contacts.get(2*skip).depth - depth; if (d4 > 0) { DContactGeom c3 = contacts.get(3*skip); c3.pos.set( contacts.get(1*skip).pos ).add( contacts.get(2*skip).pos).sub(p); c3.depth = d4; ret.inc(); DContactGeom contact = contacts.get(i*skip); contact.g1 = o1; contact.g2 = o2;
double d4 = contacts.get(1*skip).depth + contacts.get(2*skip).depth - depth; if (d4 > 0) { DContactGeom c3 = contacts.get(3*skip); c3.pos.set( contacts.get(1*skip).pos ).add( contacts.get(2*skip).pos).sub(p); c3.depth = d4; ret.inc(); DContactGeom contact = contacts.get(i*skip); contact.g1 = o1; contact.g2 = o2;
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)); }
int dCollideRaySphere (DxRay ray, DxSphere sphere, int flags, DContactGeomBuffer contacts, int skip) { dIASSERT (skip >= 1);//(int)sizeof(dContactGeom)); // dIASSERT (o1.type == dRayClass); // dIASSERT (o2.type == dSphereClass); dIASSERT ((flags & NUMC_MASK) >= 1); // dxRay *ray = (dxRay*) o1; // dxSphere *sphere = (dxSphere*) o2; DContactGeom contact = contacts.get(0); contact.g1 = ray; contact.g2 = sphere; contact.side1 = -1; contact.side2 = -1; return ray_sphere_helper (ray,sphere.final_posr().pos(),sphere.getRadius(),contacts,false); }
int dCollideRaySphere (DxRay ray, DxSphere sphere, int flags, DContactGeomBuffer contacts, int skip) { dIASSERT (skip >= 1);//(int)sizeof(dContactGeom)); // dIASSERT (o1.type == dRayClass); // dIASSERT (o2.type == dSphereClass); dIASSERT ((flags & NUMC_MASK) >= 1); // dxRay *ray = (dxRay*) o1; // dxSphere *sphere = (dxSphere*) o2; DContactGeom contact = contacts.get(0); contact.g1 = ray; contact.g2 = sphere; contact.side1 = -1; contact.side2 = -1; return ray_sphere_helper (ray,sphere.final_posr().pos(),sphere.getRadius(),contacts,false); }
int dCollideSphereSphere (DxSphere sphere1, DxSphere sphere2, int flags, DContactGeomBuffer contacts, int skip) { //dIASSERT (skip >= (int)sizeof(dContactGeom)); //TZ dIASSERT(skip ==1); // dIASSERT (o1.type == dSphereClass); // dIASSERT (o2.type == dSphereClass); dIASSERT ((flags & NUMC_MASK) >= 1); // dxSphere sphere1 = (dxSphere) o1; // dxSphere sphere2 = (dxSphere) o2; DContactGeom contact = contacts.get(0); contact.g1 = sphere1; contact.g2 = sphere2; contact.side1 = -1; contact.side2 = -1; return DxCollisionUtil.dCollideSpheres (sphere1.final_posr().pos(), sphere1._radius, sphere2.final_posr().pos(), sphere2._radius, contacts); }
private final boolean BAR2(final int ctact, final int side, final int sideinc, RefDouble depth, RefInt ret, DContactGeomBuffer contacts, final int skip, double[] A, double[] B, DxGeom o1, DxGeom o2, DVector3 p, DMatrix3C R, DVector3 boxSide, final int maxc, DVector3C n) { // if (depth - B ## sideinc < 0) goto done; \ if (depth.get()-B[sideinc-1] < 0) { done(ret, contacts, skip, o1, o2, maxc, depth.get(), p, n); return false; } // if (A ## sideinc > 0) { FOO(ctact,side,+); } else { FOO2(ctact,side,-); } \ if (A[sideinc-1] > 0) { FOO2(ctact,side, +1, contacts, skip, p, R, boxSide); } else { FOO2(ctact,side, -1, contacts, skip, p, R, boxSide); } // CONTACT(contact,ctact*skip).depth = depth; \ contacts.get(ctact*skip).depth = depth.get() - B[sideinc-1]; // ret++; ret.inc(); return true; }
int dCollideSphereSphere (DxSphere sphere1, DxSphere sphere2, int flags, DContactGeomBuffer contacts, int skip) { //dIASSERT (skip >= (int)sizeof(dContactGeom)); //TZ dIASSERT(skip ==1); // dIASSERT (o1.type == dSphereClass); // dIASSERT (o2.type == dSphereClass); dIASSERT ((flags & NUMC_MASK) >= 1); // dxSphere sphere1 = (dxSphere) o1; // dxSphere sphere2 = (dxSphere) o2; DContactGeom contact = contacts.get(0); contact.g1 = sphere1; contact.g2 = sphere2; contact.side1 = -1; contact.side2 = -1; return DxCollisionUtil.dCollideSpheres (sphere1.final_posr().pos(), sphere1._radius, sphere2.final_posr().pos(), sphere2._radius, contacts); }
private final boolean BAR2(final int ctact, final int side, final int sideinc, RefDouble depth, RefInt ret, DContactGeomBuffer contacts, final int skip, double[] A, double[] B, DxGeom o1, DxGeom o2, DVector3 p, DMatrix3C R, DVector3 boxSide, final int maxc, DVector3C n) { // if (depth - B ## sideinc < 0) goto done; \ if (depth.get()-B[sideinc-1] < 0) { done(ret, contacts, skip, o1, o2, maxc, depth.get(), p, n); return false; } // if (A ## sideinc > 0) { FOO(ctact,side,+); } else { FOO2(ctact,side,-); } \ if (A[sideinc-1] > 0) { FOO2(ctact,side, +1, contacts, skip, p, R, boxSide); } else { FOO2(ctact,side, -1, contacts, skip, p, R, boxSide); } // CONTACT(contact,ctact*skip).depth = depth; \ contacts.get(ctact*skip).depth = depth.get() - B[sideinc-1]; // ret++; ret.inc(); return true; }
private static int addContact(DGeom o1, DGeom o2, ccd_vec3_t axis, DContactGeomBuffer contacts, ccd_vec3_t p, double normaldir, double depth, int j) { DContactGeom contact = contacts.get(j++); contact.g1 = o1; contact.g2 = o2; contact.side1 = -1; contact.side2 = -1; contact.normal.set0(normaldir * axis.get0()); contact.normal.set1(normaldir * axis.get1()); contact.normal.set2(normaldir * axis.get2()); contact.depth = depth; contact.pos.set(p.get0(), p.get1(), p.get2()); return j; }
private static int addContact(DGeom o1, DGeom o2, ccd_vec3_t axis, DContactGeomBuffer contacts, ccd_vec3_t p, double normaldir, double depth, int j) { DContactGeom contact = contacts.get(j++); contact.g1 = o1; contact.g2 = o2; contact.side1 = -1; contact.side2 = -1; contact.normal.set0(normaldir * axis.get0()); contact.normal.set1(normaldir * axis.get1()); contact.normal.set2(normaldir * axis.get2()); contact.depth = depth; contact.pos.set(p.get0(), p.get1(), p.get2()); return j; }
private void nearCallback (Object data, DGeom o1, DGeom o2) { int n; final int N = 100; //dContactGeom contact[N]; DContactGeomBuffer contacts = new DContactGeomBuffer(N); if (o2 instanceof DRay) { n = OdeHelper.collide (o2,o1,N,contacts);//,sizeof(dContactGeom)); } else { n = OdeHelper.collide (o1,o2,N,contacts);//,sizeof(dContactGeom)); } if (n > 0) { DMatrix3 RI = new DMatrix3(); RI.setIdentity(); DVector3 ss = new DVector3(0.01,0.01,0.01); for (int i=0; i<n; i++) { DContactGeom contact = contacts.get(i); contact.pos.add2( Z_OFFSET ); dsDrawBox (contact.pos,RI,ss); DVector3 n2 = new DVector3(); //for (j=0; j<3; j++) n[j] = contact[i].pos[j] + 0.1*contact[i].normal[j]; n2.eqSum(contact.pos, contact.normal, 0.1); dsDrawLine (contact.pos,n2); } } }
private void nearCallback (Object data, DGeom o1, DGeom o2) { int i,n; final int N = 100; //dContactGeom contact[N]; DContactGeomBuffer contacts = new DContactGeomBuffer(N); if (o2 instanceof DRay) { n = dCollide (o2,o1,N,contacts);//,sizeof(dContactGeom)); } else { n = dCollide (o1,o2,N,contacts);//,sizeof(dContactGeom)); } if (n > 0) { DMatrix3 RI = new DMatrix3(); RI.setIdentity();//dRSetIdentity (RI); DVector3 ss = new DVector3(0.01,0.01,0.01); for (i=0; i<n; i++) { DContactGeom contact = contacts.get(i); contact.pos.add(2, Z_OFFSET ); dsDrawBox (contact.pos,RI,ss); DVector3 n2 = new DVector3(); //for (j=0; j<3; j++) n[j] = contact[i].pos[j] + 0.1*contact[i].normal[j]; n2.eqSum(contact.pos, contact.normal, 0.1); dsDrawLine (contact.pos,n2); } } }