RefInt BlocksP = new RefInt(1); double MinX = Center.get(AXIS0) - Extents.get(AXIS0); double MaxX = dNextAfter((Center.get(AXIS0) + Extents.get(AXIS0)), dInfinity); double MinZ = Center.get(AXIS1) - Extents.get(AXIS1); double MaxZ = dNextAfter((Center.get(AXIS1) + Extents.get(AXIS1)), dInfinity); this.Blocks[0].Create(MinX, MaxX, MinZ, MaxZ, null, Depth, BlocksA, BlocksP);
double lspeed = lvel.get0()*lvel.get0()+lvel.get(1)*lvel.get1()+lvel.get2()*lvel.get2(); if (lspeed > DISABLE_THRESHOLD) disable = false;
RefInt BlocksP = new RefInt(1); double MinX = Center.get(AXIS0) - Extents.get(AXIS0); double MaxX = dNextAfter((Center.get(AXIS0) + Extents.get(AXIS0)), dInfinity); double MinZ = Center.get(AXIS1) - Extents.get(AXIS1); double MaxZ = dNextAfter((Center.get(AXIS1) + Extents.get(AXIS1)), dInfinity); this.Blocks[0].Create(MinX, MaxX, MinZ, MaxZ, null, Depth, BlocksA, BlocksP);
private static void answer(final double t, DVector3 tmp, DVector3C sign, DVector3C p1, DVector3 s, DMatrix3C R, DVector3 lret, DVector3C h, DVector3C c, DVector3C v, DVector3 bret) { //got_answer: // compute closest point on the line //for (int i=0; i<3; i++) lret.v[i] = p1.v[i] + t*tmp.v[i]; // note: tmp=p2-p1 lret.eqSum(p1, tmp, t); // compute closest point on the box for (int i=0; i<3; i++) { tmp.set(i, sign.get(i) * (s.get(i) + t*v.get(i)) ); if (tmp.get(i) < -h.get(i)) tmp.set(i, -h.get(i) ); else if (tmp.get(i) > h.get(i)) tmp.set(i, h.get(i) ); } dMultiply0_331 (s,R,tmp); //for (int i=0; i<3; i++) bret.v[i] = s.v[i] + c.v[i]; bret.eqSum(s, c); }
private static void answer(final double t, DVector3 tmp, DVector3C sign, DVector3C p1, DVector3 s, DMatrix3C R, DVector3 lret, DVector3C h, DVector3C c, DVector3C v, DVector3 bret) { //got_answer: // compute closest point on the line //for (int i=0; i<3; i++) lret.v[i] = p1.v[i] + t*tmp.v[i]; // note: tmp=p2-p1 lret.eqSum(p1, tmp, t); // compute closest point on the box for (int i=0; i<3; i++) { tmp.set(i, sign.get(i) * (s.get(i) + t*v.get(i)) ); if (tmp.get(i) < -h.get(i)) tmp.set(i, -h.get(i) ); else if (tmp.get(i) > h.get(i)) tmp.set(i, h.get(i) ); } dMultiply0_331 (s,R,tmp); //for (int i=0; i<3; i++) bret.v[i] = s.v[i] + c.v[i]; bret.eqSum(s, c); }
m.setBox (1, boxDim.get(X), boxDim.get(Y), boxDim.get(Z)); m.adjust (Mass1); geomW = OdeHelper.createBox (space, boxDim.get(X), boxDim.get(Y), boxDim.get(Z)); geomW.setBody (bodyW); geomW.setCategoryBits (catBits[W]); m.setBox (1, boxDim.get(X), boxDim.get(Y), boxDim.get(Z)); m.adjust (Mass1); geomD = OdeHelper.createBox (space, boxDim.get(X), boxDim.get(Y), boxDim.get(Z)); geomD.setBody (bodyD); geomD.setCategoryBits (catBits[D]); geomEXT = OdeHelper.createBox (null, extDim.get(X), extDim.get(Y), extDim.get(Z)); geomEXT.setCategoryBits (catBits[EXT]); geomEXT.setCollideBits ( geomINT = OdeHelper.createBox (null, INT_EXT_RATIO*extDim.get(X), INT_EXT_RATIO*extDim.get(Y), INT_EXT_RATIO*extDim.get(Z)); geomINT.setCategoryBits (catBits[INT]); geomINT.setCollideBits ( geomANCHOR = OdeHelper.createBox (null, ancDim.get(X), ancDim.get(Y), ancDim.get(Z)); geomANCHOR.setCategoryBits (catBits[ANCHOR]); geomANCHOR.setCollideBits (
m.setBox (1, boxDim.get(X), boxDim.get(Y), boxDim.get(Z)); m.adjust (Mass1); geom[W] = dCreateBox (space, boxDim.get(X), boxDim.get(Y), boxDim.get(Z)); dGeomSetBody (geom[W], body[W]); dGeomSetCategoryBits (geom[W], catBits[W]); m.setBox (1, boxDim.get(X), boxDim.get(Y), boxDim.get(Z)); m.adjust (Mass1); geom[D] = dCreateBox (space, boxDim.get(X), boxDim.get(Y), boxDim.get(Z)); dGeomSetBody (geom[D], body[D]); dGeomSetCategoryBits (geom[D], catBits[D]); geom[EXT] = dCreateBox (null, extDim.get(X), extDim.get(Y), extDim.get(Z)); dGeomSetCategoryBits (geom[EXT], catBits[EXT]); dGeomSetCollideBits (geom[EXT], geom[INT] = dCreateBox (null, INT_EXT_RATIO*extDim.get(X), INT_EXT_RATIO*extDim.get(Y), INT_EXT_RATIO*extDim.get(Z)); dGeomSetCategoryBits (geom[INT], catBits[INT]); dGeomSetCollideBits (geom[INT], geom[ANCHOR] = dCreateBox (null, ancDim.get(X), ancDim.get(Y), ancDim.get(Z)); dGeomSetCategoryBits (geom[ANCHOR], catBits[ANCHOR]); dGeomSetCollideBits (geom[ANCHOR],
DVector3 sizeP = new DVector3(0, 0.1, 0.1); for (int i=0; i<3; ++i) sizeP.add(0, (anchorPos.get(i) - posBox1.get(i))*(anchorPos.get(i) - posBox1.get(i)) ); sizeP.set(0, Math.sqrt(sizeP.get(0))); DVector3 posAxisP = new DVector3(); for (int i=0; i<3; ++i) posAxisP.set(i, posBox1.get(i) + (anchorPos.get(i) - posBox1.get(i))/2.0 ); dsDrawBox (posAxisP, rotBox1, sizeP); DVector3 sizeR = new DVector3(0, 0.1, 0.1); for (int i=0; i<3; ++i) sizeR.add(0, (anchorPos.get(i) - posBox2.get(i))*(anchorPos.get(i) - posBox2.get(i)) ); sizeR.set(0, Math.sqrt(sizeR.get(0))); DVector3 posAxisR = new DVector3(); for (int i=0; i<3; ++i) posAxisR.set(i, posBox2.get(i) + (anchorPos.get(i) - posBox2.get(i))/2.0 ); dsDrawBox (posAxisR, rotBox2, sizeR);
DVector3 sizeP = new DVector3(0, 0.1, 0.1); for (int i=0; i<3; ++i) sizeP.add(0, (anchorPos.get(i) - posBox1.get(i))*(anchorPos.get(i) - posBox1.get(i)) ); sizeP.set(0, sqrt(sizeP.get(0))); DVector3 posAxisP = new DVector3(); for (int i=0; i<3; ++i) posAxisP.set(i, posBox1.get(i) + (anchorPos.get(i) - posBox1.get(i))/2.0 ); dsDrawBox (posAxisP, rotBox1, sizeP); DVector3 sizeR = new DVector3(0, 0.1, 0.1); for (int i=0; i<3; ++i) sizeR.add(0, (anchorPos.get(i) - posBox2.get(i))*(anchorPos.get(i) - posBox2.get(i)) ); sizeR.set(0, sqrt(sizeR.get(0))); DVector3 posAxisR = new DVector3(); for (int i=0; i<3; ++i) posAxisR.set(i, posBox2.get(i) + (anchorPos.get(i) - posBox2.get(i))/2.0 ); dsDrawBox (posAxisR, rotBox2, sizeR);
for (k=0; k<num; k++) { final DVector3C pos = dBodyGetPosition (obj[k].body); if (pos.get(2) > maxheight) maxheight = pos.get(2); dGeomSetBody (obj[i].geom[k],obj[i].body); dGeomSetOffsetPosition (obj[i].geom[k], dpos[k][0]-m_c.get(0), dpos[k][1]-m_c.get(1), dpos[k][2]-m_c.get(2)); dGeomSetOffsetRotation(obj[i].geom[k], drot[k]); dMassTranslate (m,-m_c.get(0),-m_c.get(1),-m_c.get(2)); dBodySetMass (obj[i].body,m); final DVector3C pos = dGeomGetPosition(obj[selected].geom[0]); final DMatrix3C rot = dGeomGetRotation(obj[selected].geom[0]); printf("POSITION:\n\t[%f,%f,%f]\n\n",pos.get(0),pos.get(1),pos.get(2)); printf("ROTATION:\n\t[%f,%f,%f,%f]\n\t[%f,%f,%f,%f]\n\t[%f,%f,%f,%f]\n\n",
if (nr.get(lanr) < 0) { for (i=0; i<3; i++) center.set(i, pb.get(i) - pa.get(i) + Sb.get(lanr) * Rb.get(i, lanr) ); for (i=0; i<3; i++) center.set(i, pb.get(i) - pa.get(i) - Sb.get(lanr) * Rb.get(i, lanr) ); m22 = dCalcVectorDot3_44 (Ra,code2,Rb,a2); double k1 = m11*Sb.get(a1); double k2 = m21*Sb.get(a1); double k3 = m12*Sb.get(a2); double k4 = m22*Sb.get(a2); quad[0] = c1 - k1 - k3; quad[1] = c2 - k2 - k4; rect[0] = Sa.get(code1); rect[1] = Sa.get(code2); dep[cnum] = Sa.get(codeN) - dCalcVectorDot3(normal2, point,cnum*3); if (dep[cnum] >= 0) { ret[cnum*2] = ret[j*2]; for (i=0; i<3; i++) con.pos.set(i, point[j*3+i] + pa.get(i) ); con.depth = dep[j]; for (i=0; i<3; i++) con.pos.set(i, point[iret[j]*3+i] + pa.get(i) ); con.depth = dep[iret[j]];
if (nr.get(lanr) < 0) { for (i=0; i<3; i++) center.set(i, pb.get(i) - pa.get(i) + Sb.get(lanr) * Rb.get(i, lanr) ); for (i=0; i<3; i++) center.set(i, pb.get(i) - pa.get(i) - Sb.get(lanr) * Rb.get(i, lanr) ); m22 = dCalcVectorDot3_44 (Ra,code2,Rb,a2); double k1 = m11*Sb.get(a1); double k2 = m21*Sb.get(a1); double k3 = m12*Sb.get(a2); double k4 = m22*Sb.get(a2); quad[0] = c1 - k1 - k3; quad[1] = c2 - k2 - k4; rect[0] = Sa.get(code1); rect[1] = Sa.get(code2); dep[cnum] = Sa.get(codeN) - dCalcVectorDot3(normal2, point,cnum*3); if (dep[cnum] >= 0) { ret[cnum*2] = ret[j*2]; for (i=0; i<3; i++) con.pos.set(i, point[j*3+i] + pa.get(i) ); con.depth = dep[j]; for (i=0; i<3; i++) con.pos.set(i, point[iret[j]*3+i] + pa.get(i) ); con.depth = dep[iret[j]];
pos.eqSum(anchorPos, axisP, -0.5 * extDim.get(Z)); dsDrawBox (pos, rotBox, l);