/** * @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 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; }
/** * @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 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 void testCrossProduct() { HEADER(); DVector3 a1 = new DVector3(),a2 = new DVector3(); DVector3 b = new DVector3(),c = new DVector3(); DMatrix3 B = new DMatrix3(); dMakeRandomVector (b,1.0); dMakeRandomVector (c,1.0); a1.eqCross(b,c); //B.dSetZero();//dSetZero (B,12); dSetCrossMatrixPlus (B,b); dMultiply0 (a2,B,c); double diff = dMaxDifference(a1,a2); println ("\t", diff > tol ? "FAILED" : "passed"); }
m_vN.eqCross(m_vE0, m_vE1); vL.eqCross(vA0, m_vE0); fD = vL.dot(m_vN)/fNLen; fp0 = vL.dot(vD); vL.eqCross(vA0, m_vE1); fD = vL.dot(m_vN)/fNLen; fp0 = vL.dot(vD); vL.eqCross(vA0, m_vE2); fD = vL.dot(m_vN)/fNLen; fp0 = vL.dot(vD); vL.eqCross(vA1, m_vE0); fD = vL.dot(m_vN)/fNLen; fp0 = vL.dot(vD); vL.eqCross(vA1, m_vE1); fD = vL.dot(m_vN)/fNLen; fp0 = vL.dot(vD); vL.eqCross(vA1, m_vE2); fD = vL.dot(m_vN)/fNLen; fp0 = vL.dot(vD); vL.eqCross(vA2, m_vE0); fD = vL.dot(m_vN)/fNLen; fp0 = vL.dot(vD);
m_vN.eqCross(m_vE0, m_vE1); vL.eqCross(vA0, m_vE0); fD = vL.dot(m_vN)/fNLen; fp0 = vL.dot(vD); vL.eqCross(vA0, m_vE1); fD = vL.dot(m_vN)/fNLen; fp0 = vL.dot(vD); vL.eqCross(vA0, m_vE2); fD = vL.dot(m_vN)/fNLen; fp0 = vL.dot(vD); vL.eqCross(vA1, m_vE0); fD = vL.dot(m_vN)/fNLen; fp0 = vL.dot(vD); vL.eqCross(vA1, m_vE1); fD = vL.dot(m_vN)/fNLen; fp0 = vL.dot(vD); vL.eqCross(vA1, m_vE2); fD = vL.dot(m_vN)/fNLen; fp0 = vL.dot(vD); vL.eqCross(vA2, m_vE0); fD = vL.dot(m_vN)/fNLen; fp0 = vL.dot(vD);
m_vN.eqCross(m_vE0, m_vE1); vL.eqCross(vA0, m_vE0); fD = vL.dot(m_vN)/fNLen; fp0 = vL.dot(vD); vL.eqCross(vA0, m_vE1); fD = vL.dot(m_vN)/fNLen; fp0 = vL.dot(vD); vL.eqCross(vA0, m_vE2); fD = vL.dot(m_vN)/fNLen; fp0 = vL.dot(vD); vL.eqCross(vA1, m_vE0); fD = vL.dot(m_vN)/fNLen; fp0 = vL.dot(vD); vL.eqCross(vA1, m_vE1); fD = vL.dot(m_vN)/fNLen; fp0 = vL.dot(vD); vL.eqCross(vA1, m_vE2); fD = vL.dot(m_vN)/fNLen; fp0 = vL.dot(vD); vL.eqCross(vA2, m_vE0); fD = vL.dot(m_vN)/fNLen; fp0 = vL.dot(vD);
m_vN.eqCross(m_vE0, m_vE1); vL.eqCross(vA0, m_vE0); fD = vL.dot(m_vN)/fNLen; fp0 = vL.dot(vD); vL.eqCross(vA0, m_vE1); fD = vL.dot(m_vN)/fNLen; fp0 = vL.dot(vD); vL.eqCross(vA0, m_vE2); fD = vL.dot(m_vN)/fNLen; fp0 = vL.dot(vD); vL.eqCross(vA1, m_vE0); fD = vL.dot(m_vN)/fNLen; fp0 = vL.dot(vD); vL.eqCross(vA1, m_vE1); fD = vL.dot(m_vN)/fNLen; fp0 = vL.dot(vD); vL.eqCross(vA1, m_vE2); fD = vL.dot(m_vN)/fNLen; fp0 = vL.dot(vD); vL.eqCross(vA2, m_vE0); fD = vL.dot(m_vN)/fNLen; fp0 = vL.dot(vD);
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); }
u2.normalize(); if (dFabs(u1.dot(u2)) > 1e-6) dDebug (0,"bad u1/u2"); n.eqCross(u1, u2);