@Override public double getPointDepth(DVector3C p) { return _pD - _pV.dot(p); }
@Override public double getPointDepth(DVector3C p) { return _pD - _pV.dot(p); }
double dJointGetAMotorAngleRate( int anum ) { dAASSERT( anum >= 0 && anum < 3); if (node[0].body != null) { DVector3 axis = new DVector3(); dJointGetAMotorAxis (anum, axis); double rate = axis.dot( node[0].body.avel ); if (node[1].body!=null) rate -= axis.dot( node[1].body.avel ); return rate; } return 0; }
double dJointGetAMotorAngleRate( int anum ) { dAASSERT( anum >= 0 && anum < 3); if (node[0].body != null) { DVector3 axis = new DVector3(); dJointGetAMotorAxis (anum, axis); double rate = axis.dot( node[0].body.avel ); if (node[1].body!=null) rate -= axis.dot( node[1].body.avel ); return rate; } return 0; }
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"); }
double d1 = dSqrt(u1.dot(u1)); double d2 = dSqrt(u2.dot(u2)); u1.normalize(); u2.normalize(); if (dFabs(u1.dot(u2)) > 1e-6) dDebug (0,"bad u1/u2"); n.eqCross(u1, u2); double d = -n.dot(p1); if (dFabs(n.dot(p1)+d) > 1e-8) dDebug (0,"bad n wrt p1"); if (dFabs(n.dot(p2)+d) > 1e-8) dDebug (0,"bad n wrt p2"); if (dFabs(n.dot(p3)+d) > 1e-8) dDebug (0,"bad n wrt p3"); double alpha = -(d+n.dot(v1))/n.dot(tmp); if (dFabs(n.dot(tmp)+d) > 1e-6) dDebug (0,"bad tmp"); if (alpha < 0) return false; if (alpha > 1) return false; double a1 = u1.dot(tmp); double a2 = u2.dot(tmp); if (a1<0 || a2<0 || a1>d1 || a2>d2) return false; return true;
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 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; }
/** * Function that computes ax1,ax2 = axis 1 and 2 in global coordinates (they are * relative to body 1 and 2 initially) and then computes the constrained * rotational axis as the cross product of ax1 and ax2. * the sin and cos of the angle between axis 1 and 2 is computed, this comes * from dot and cross product rules. * * @param ax1 Will contain the joint axis1 in world frame * @param ax2 Will contain the joint axis2 in world frame * @param axis Will contain the cross product of ax1 x ax2 * @param sin_angle * @param cos_angle */ private void getAxisInfo(DVector3 ax1, DVector3 ax2, DVector3 axCross, RefDouble sin_angle, RefDouble cos_angle) { dMultiply0_331 (ax1, node[0].body.posr().R(), _axis1); dMultiply0_331 (ax2, node[1].body.posr().R(), _axis2); dCalcVectorCross3 (axCross ,ax1, ax2); sin_angle.d = axCross.length();//dSqrt (axCross[0]*axCross[0] + axCross[1]*axCross[1] + axCross[2]*axCross[2]); cos_angle.d = ax1.dot(ax2);//dDOT (ax1,ax2); }
private static int GetSupportSide(DVector3 dir,DxConvex cvx) { DVector3 dics = new DVector3(),tmp = new DVector3(); // Direction in convex space double SavedDot; double Dot; //unsigned int side=0; //dVector3Copy(dir,tmp); tmp.set(dir); dNormalize3(tmp); dMultiply1_331(dics,cvx.final_posr().R(),tmp); SavedDot = dics.dot(cvx.planesV[0]); for(int i=1;i<cvx.planecount;++i) { Dot = dCalcVectorDot3(dics,cvx.planesV[i]);//+(i*4)); if(Dot>SavedDot) { SavedDot=Dot; side=i; } } return side; }
/** * Function that computes ax1,ax2 = axis 1 and 2 in global coordinates (they are * relative to body 1 and 2 initially) and then computes the constrained * rotational axis as the cross product of ax1 and ax2. * the sin and cos of the angle between axis 1 and 2 is computed, this comes * from dot and cross product rules. * * @param ax1 Will contain the joint axis1 in world frame * @param ax2 Will contain the joint axis2 in world frame * @param axis Will contain the cross product of ax1 x ax2 * @param sin_angle * @param cos_angle */ private void getAxisInfo(DVector3 ax1, DVector3 ax2, DVector3 axCross, RefDouble sin_angle, RefDouble cos_angle) { dMultiply0_331 (ax1, node[0].body.posr().R(), _axis1); dMultiply0_331 (ax2, node[1].body.posr().R(), _axis2); dCalcVectorCross3 (axCross ,ax1, ax2); sin_angle.d = axCross.length();//dSqrt (axCross[0]*axCross[0] + axCross[1]*axCross[1] + axCross[2]*axCross[2]); cos_angle.d = ax1.dot(ax2);//dDOT (ax1,ax2); }
public double dJointGetPRPositionRate( ) { // dxJointPR joint = ( dxJointPR ) j; // dUASSERT( joint, "bad joint argument" ); // checktype( joint, dxJointPR.class ); // get axis1 in global coordinates DVector3 ax1 = new DVector3(); dMultiply0_331( ax1, node[0].body.posr().R(), axisP1 ); if ( node[1].body!=null ) { DVector3 lv2 = new DVector3(); node[1].body.dBodyGetRelPointVel( _anchor2, lv2 ); return dCalcVectorDot3( ax1, node[0].body.lvel ) - dCalcVectorDot3( ax1, lv2 ); } else { double rate = ax1.dot( node[0].body.lvel ); return ( isFlagsReverse() ? -rate : rate); } }
private static int GetSupportSide(DVector3 dir,DxConvex cvx) { DVector3 dics = new DVector3(),tmp = new DVector3(); // Direction in convex space double SavedDot; double Dot; //unsigned int side=0; //dVector3Copy(dir,tmp); tmp.set(dir); dNormalize3(tmp); dMultiply1_331(dics,cvx.final_posr().R(),tmp); SavedDot = dics.dot(cvx.planesV[0]); for(int i=1;i<cvx.planecount;++i) { Dot = dCalcVectorDot3(dics,cvx.planesV[i]);//+(i*4)); if(Dot>SavedDot) { SavedDot=Dot; side=i; } } return side; }
public double dJointGetPRPositionRate( ) { // dxJointPR joint = ( dxJointPR ) j; // dUASSERT( joint, "bad joint argument" ); // checktype( joint, dxJointPR.class ); // get axis1 in global coordinates DVector3 ax1 = new DVector3(); dMultiply0_331( ax1, node[0].body.posr().R(), axisP1 ); if ( node[1].body!=null ) { DVector3 lv2 = new DVector3(); node[1].body.dBodyGetRelPointVel( _anchor2, lv2 ); return dCalcVectorDot3( ax1, node[0].body.lvel ) - dCalcVectorDot3( ax1, lv2 ); } else { double rate = ax1.dot( node[0].body.lvel ); return ( isFlagsReverse() ? -rate : rate); } }
/** * @param baseAxis - base axis for constraints * @param body2Axis - main axis of the 2nd body */ public void setAxes(DVector3 baseAxis, DVector3 body2Axis) { baseAxis.safeNormalize(); body2Axis.safeNormalize(); DVector3 twistUpAxis = new DVector3(); if (Math.abs(body2Axis.dot(new DVector3(0.0, 1.0, 0.0))) < 0.7) { twistUpAxis.set(0, 1, 0); } else { twistUpAxis.set(0, 0, 1); } DVector3 cross = new DVector3(); cross.eqCross(body2Axis, twistUpAxis); cross.safeNormalize(); twistUpAxis.eqCross(cross, body2Axis); twistUpAxis.safeNormalize(); setAxes(baseAxis, this.baseAxis, null); setAxes(body2Axis, null, this.body2Axis); setAxes(twistUpAxis, twistUpAxis1, twistUpAxis2); }
/** * @param baseAxis - base axis for constraints * @param body2Axis - main axis of the 2nd body */ public void setAxes(DVector3 baseAxis, DVector3 body2Axis) { baseAxis.safeNormalize(); body2Axis.safeNormalize(); DVector3 twistUpAxis = new DVector3(); if (Math.abs(body2Axis.dot(new DVector3(0.0, 1.0, 0.0))) < 0.7) { twistUpAxis.set(0, 1, 0); } else { twistUpAxis.set(0, 0, 1); } DVector3 cross = new DVector3(); cross.eqCross(body2Axis, twistUpAxis); cross.safeNormalize(); twistUpAxis.eqCross(cross, body2Axis); twistUpAxis.safeNormalize(); setAxes(baseAxis, this.baseAxis, null); setAxes(body2Axis, null, this.body2Axis); setAxes(twistUpAxis, twistUpAxis1, twistUpAxis2); }
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); }
private void makeV1andV2() { if ( node[0].body != null) { // get axis 1 and 2 in global coords DVector3 ax1 = new DVector3(), ax2 = new DVector3(), v = new DVector3(); dMultiply0_331( ax1, node[0].body.posr().R(), _axis1 ); dMultiply0_331( ax2, node[1].body.posr().R(), _axis2 ); // don't do anything if the axis1 or axis2 vectors are zero or the same if (( ax1.get0() == 0 && ax1.get1() == 0 && ax1.get2() == 0 ) || ( ax2.get0() == 0 && ax2.get1() == 0 && ax2.get2() == 0 ) || ( ax1.get0() == ax2.get0() && ax1.get1() == ax2.get1() && ax1.get2() == ax2.get2() ) ) return; // modify axis 2 so it's perpendicular to axis 1 double k = ax1.dot( ax2 ); //for ( int i = 0; i < 3; i++ ) ax2.v[i] -= k * ax1.v[i]; ax2.eqSum( ax2, ax1, -k); dNormalize3( ax2 ); // make v1 = modified axis2, v2 = axis1 x (modified axis2) dCalcVectorCross3( v, ax1, ax2 ); dMultiply1_331( v1, node[0].body.posr().R(), ax2 ); dMultiply1_331( v2, node[0].body.posr().R(), v ); } }
private void makeV1andV2() { if ( node[0].body != null) { // get axis 1 and 2 in global coords DVector3 ax1 = new DVector3(), ax2 = new DVector3(), v = new DVector3(); dMultiply0_331( ax1, node[0].body.posr().R(), _axis1 ); dMultiply0_331( ax2, node[1].body.posr().R(), _axis2 ); // don't do anything if the axis1 or axis2 vectors are zero or the same if (( ax1.get0() == 0 && ax1.get1() == 0 && ax1.get2() == 0 ) || ( ax2.get0() == 0 && ax2.get1() == 0 && ax2.get2() == 0 ) || ( ax1.get0() == ax2.get0() && ax1.get1() == ax2.get1() && ax1.get2() == ax2.get2() ) ) return; // modify axis 2 so it's perpendicular to axis 1 double k = ax1.dot( ax2 ); //for ( int i = 0; i < 3; i++ ) ax2.v[i] -= k * ax1.v[i]; ax2.eqSum( ax2, ax1, -k); dNormalize3( ax2 ); // make v1 = modified axis2, v2 = axis1 x (modified axis2) dCalcVectorCross3( v, ax1, ax2 ); dMultiply1_331( v1, node[0].body.posr().R(), ax2 ); dMultiply1_331( v2, node[0].body.posr().R(), v ); } }