/** * check whether an n*n matrix A is positive definite, return 1/0 (yes/no). * positive definite means that x'*A*x > 0 for any x. this performs a * cholesky decomposition of A. if the decomposition fails then the matrix * is not positive definite. A is stored by rows. A is not altered. * @param A A * @return 'false' on failure */ public static boolean dIsPositiveDefinite(DMatrix3C A) { return dFactorCholesky(A.clone()); }
/** * return the maximum element difference between the two 3*3 matrices. * @param A A * @param B B * @return difference */ public static double dMaxDifference (final DMatrix3C A, final DMatrix3C B) { double diff,max; max = 0; for (int i=0; i<3; i++) { for (int j=0; j<3; j++) { diff = Math.abs(A.get(i,j) - B.get(i,j)); if (diff > max) { max = diff; } } } return max; } /**
public static double dCalcMatrix3Det( final DMatrix3C mat ) { double det; // det = mat[0] * ( mat[5]*mat[10] - mat[9]*mat[6] ) // - mat[1] * ( mat[4]*mat[10] - mat[8]*mat[6] ) // + mat[2] * ( mat[4]*mat[9] - mat[8]*mat[5] ); det = mat.get00() * ( mat.get11()*mat.get22() - mat.get21()*mat.get12() ) - mat.get01() * ( mat.get10()*mat.get22() - mat.get20()*mat.get12() ) + mat.get02() * ( mat.get10()*mat.get21() - mat.get20()*mat.get11() ); return( det ); }
public static void dSubtractVectorCross3(DVector3 a, DVector3C b, DMatrix3C c) { a.add0( - (b.get1()*c.get02() - b.get2()*c.get01()) ); a.add1( - (b.get2()*c.get00() - b.get0()*c.get02()) ); a.add2( - (b.get0()*c.get01() - b.get1()*c.get00()) ); } // public static void dCROSS(DVector3 a, OP op, DVector3C b, DMatrix3C c) {
y.set0((b.get(0) - sum) / L.get00()); sum = L.get10() * y.get0(); y.set1((b.get(1) - sum) / L.get11()); sum = L.get20() * y.get0() + L.get21() * y.get1(); y.set2((b.get(2) - sum) / L.get22()); b.set2((y.get2() - sum) / L.get22()); sum = L.get21() * b.get2(); b.set1((y.get1() - sum) / L.get11()); sum = L.get10() * b.get1() + L.get20() * b.get2(); b.set0((y.get0() - sum) / L.get00());
@Override void computeAABB() { final DMatrix3C R = final_posr().R(); final DVector3C pos = final_posr().pos(); double dOneMinusR2Square = (1.0 - R.get02()*R.get02()); double xrange = dFabs(R.get02()*_lz*0.5) + _radius * dSqrt(dMAX(0.0, dOneMinusR2Square)); double dOneMinusR6Square = (1.0 - R.get12()*R.get12()); double yrange = dFabs(R.get12()*_lz*0.5) + _radius * dSqrt(dMAX(0.0, dOneMinusR6Square)); double dOneMinusR10Square = (1.0 - R.get22()*R.get22()); double zrange = dFabs(R.get22()*_lz*0.5) + _radius * dSqrt(dMAX(0.0, dOneMinusR10Square)); // _aabb.v[0] = pos.v[0] - xrange; // _aabb.v[1] = pos.v[0] + xrange; // _aabb.v[2] = pos.v[1] - yrange; // _aabb.v[3] = pos.v[1] + yrange; // _aabb.v[4] = pos.v[2] - zrange; // _aabb.v[5] = pos.v[2] + zrange; _aabb.setMinMax(xrange, yrange, zrange); _aabb.shiftPos(pos); }
o1.final_posr().R().get02() * (o2.final_posr().pos().get0() - o1.final_posr().pos().get0()) + o1.final_posr().R().get12() * (o2.final_posr().pos().get1() - o1.final_posr().pos().get1()) + o1.final_posr().R().get22() * (o2.final_posr().pos().get2() - o1.final_posr().pos().get2()); double lz2 = ccyl._lz * (0.5); if (alpha > lz2) alpha = lz2; p.eqSum( o1.final_posr().pos(), o1.final_posr().R().columnAsNewVector(2), alpha); return DxCollisionUtil.dCollideSpheres (p,ccyl._radius,o2.final_posr().pos(),sphere.getRadius(),contacts);
normal.set(tst._normalR_M.viewCol(tst._normalR_col)); sign = (dCalcVectorDot3_14(normal,R1,j) > 0) ? (1.0) : (-1.0); for (i=0; i<3; i++) pa.add(i, sign * A.get(j) * R1.get(i, j) ); sign = (dCalcVectorDot3_14(normal,R2,j) > 0) ? (-1.0) : (1.0); for (i=0; i<3; i++) pb.add(i, sign * B.get(j) * R2.get(i, j) ); for (i=0; i<3; i++) ua.set(i, R1.get(i, (tst._code-7)/3 ) ); for (i=0; i<3; i++) ub.set(i, R2.get(i, (tst._code-7)%3 ) ); 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) ); for (i=0; i<3; i++) point[cnum*3+i] = center.get(i) + k1*Rb.get(i, a1) + k2*Rb.get(i, a2); dep[cnum] = Sa.get(codeN) - dCalcVectorDot3(normal2, point,cnum*3); if (dep[cnum] >= 0) {
y.set0((b.get(0) - sum) / L.get00()); sum = L.get10() * y.get0(); y.set1((b.get(1) - sum) / L.get11()); sum = L.get20() * y.get0() + L.get21() * y.get1(); y.set2((b.get(2) - sum) / L.get22()); b.set2((y.get2() - sum) / L.get22()); sum = L.get21() * b.get2(); b.set1((y.get1() - sum) / L.get11()); sum = L.get10() * b.get1() + L.get20() * b.get2(); b.set0((y.get0() - sum) / L.get00());
@Override void computeAABB() { final DMatrix3C R = final_posr().R(); final DVector3C pos = final_posr().pos(); double dOneMinusR2Square = (1.0 - R.get02()*R.get02()); double xrange = dFabs(R.get02()*_lz*0.5) + _radius * dSqrt(dMAX(0.0, dOneMinusR2Square)); double dOneMinusR6Square = (1.0 - R.get12()*R.get12()); double yrange = dFabs(R.get12()*_lz*0.5) + _radius * dSqrt(dMAX(0.0, dOneMinusR6Square)); double dOneMinusR10Square = (1.0 - R.get22()*R.get22()); double zrange = dFabs(R.get22()*_lz*0.5) + _radius * dSqrt(dMAX(0.0, dOneMinusR10Square)); // _aabb.v[0] = pos.v[0] - xrange; // _aabb.v[1] = pos.v[0] + xrange; // _aabb.v[2] = pos.v[1] - yrange; // _aabb.v[3] = pos.v[1] + yrange; // _aabb.v[4] = pos.v[2] - zrange; // _aabb.v[5] = pos.v[2] + zrange; _aabb.setMinMax(xrange, yrange, zrange); _aabb.shiftPos(pos); }
o1.final_posr().R().get02() * (o2.final_posr().pos().get0() - o1.final_posr().pos().get0()) + o1.final_posr().R().get12() * (o2.final_posr().pos().get1() - o1.final_posr().pos().get1()) + o1.final_posr().R().get22() * (o2.final_posr().pos().get2() - o1.final_posr().pos().get2()); double lz2 = ccyl._lz * (0.5); if (alpha > lz2) alpha = lz2; p.eqSum( o1.final_posr().pos(), o1.final_posr().R().columnAsNewVector(2), alpha); return DxCollisionUtil.dCollideSpheres (p,ccyl._radius,o2.final_posr().pos(),sphere.getRadius(),contacts);
public static void dSubtractVectorCross3(DVector3 a, DVector3C b, DMatrix3C c) { a.add0( - (b.get1()*c.get02() - b.get2()*c.get01()) ); a.add1( - (b.get2()*c.get00() - b.get0()*c.get02()) ); a.add2( - (b.get0()*c.get01() - b.get1()*c.get00()) ); } // public static void dCROSS(DVector3 a, OP op, DVector3C b, DMatrix3C c) {
normal.set(tst._normalR_M.viewCol(tst._normalR_col)); sign = (dCalcVectorDot3_14(normal,R1,j) > 0) ? (1.0) : (-1.0); for (i=0; i<3; i++) pa.add(i, sign * A.get(j) * R1.get(i, j) ); sign = (dCalcVectorDot3_14(normal,R2,j) > 0) ? (-1.0) : (1.0); for (i=0; i<3; i++) pb.add(i, sign * B.get(j) * R2.get(i, j) ); for (i=0; i<3; i++) ua.set(i, R1.get(i, (tst._code-7)/3 ) ); for (i=0; i<3; i++) ub.set(i, R2.get(i, (tst._code-7)%3 ) ); 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) ); for (i=0; i<3; i++) point[cnum*3+i] = center.get(i) + k1*Rb.get(i, a1) + k2*Rb.get(i, a2); dep[cnum] = Sa.get(codeN) - dCalcVectorDot3(normal2, point,cnum*3); if (dep[cnum] >= 0) {
public static double dCalcMatrix3Det( final DMatrix3C mat ) { double det; // det = mat[0] * ( mat[5]*mat[10] - mat[9]*mat[6] ) // - mat[1] * ( mat[4]*mat[10] - mat[8]*mat[6] ) // + mat[2] * ( mat[4]*mat[9] - mat[8]*mat[5] ); det = mat.get00() * ( mat.get11()*mat.get22() - mat.get21()*mat.get12() ) - mat.get01() * ( mat.get10()*mat.get22() - mat.get20()*mat.get12() ) + mat.get02() * ( mat.get10()*mat.get21() - mat.get20()*mat.get11() ); return( det ); }
@Override public void computeAABB() { final DMatrix3C R = final_posr().R(); final DVector3C pos = final_posr().pos(); // double xrange = dFabs(R.v[2] * _lz) * (0.5) + _radius; // double yrange = dFabs(R.v[6] * _lz) * (0.5) + _radius; // double zrange = dFabs(R.v[10] * _lz) * (0.5) + _radius; double xrange = dFabs(R.get02() * _lz) * (0.5) + _radius; double yrange = dFabs(R.get12() * _lz) * (0.5) + _radius; double zrange = dFabs(R.get22() * _lz) * (0.5) + _radius; // _aabb.v[0] = pos.v[0] - xrange; // _aabb.v[1] = pos.v[0] + xrange; // _aabb.v[2] = pos.v[1] - yrange; // _aabb.v[3] = pos.v[1] + yrange; // _aabb.v[4] = pos.v[2] - zrange; // _aabb.v[5] = pos.v[2] + zrange; _aabb.setMinMax(xrange, yrange, zrange); _aabb.shiftPos(pos); }
/** * check whether an n*n matrix A is positive definite, return 1/0 (yes/no). * positive definite means that x'*A*x > 0 for any x. this performs a * cholesky decomposition of A. if the decomposition fails then the matrix * is not positive definite. A is stored by rows. A is not altered. * @param A A * @return 'false' on failure */ public static boolean dIsPositiveDefinite(DMatrix3C A) { return dFactorCholesky(A.clone()); }
/** * return the maximum element difference between the two 3*3 matrices. * @param A A * @param B B * @return difference */ public static double dMaxDifference (final DMatrix3C A, final DMatrix3C B) { double diff,max; max = 0; for (int i=0; i<3; i++) { for (int j=0; j<3; j++) { diff = Math.abs(A.get(i,j) - B.get(i,j)); if (diff > max) { max = diff; } } } return max; } /**
@Override public void computeAABB() { final DMatrix3C R = final_posr().R(); final DVector3C pos = final_posr().pos(); // double xrange = dFabs(R.v[2] * _lz) * (0.5) + _radius; // double yrange = dFabs(R.v[6] * _lz) * (0.5) + _radius; // double zrange = dFabs(R.v[10] * _lz) * (0.5) + _radius; double xrange = dFabs(R.get02() * _lz) * (0.5) + _radius; double yrange = dFabs(R.get12() * _lz) * (0.5) + _radius; double zrange = dFabs(R.get22() * _lz) * (0.5) + _radius; // _aabb.v[0] = pos.v[0] - xrange; // _aabb.v[1] = pos.v[0] + xrange; // _aabb.v[2] = pos.v[1] - yrange; // _aabb.v[3] = pos.v[1] + yrange; // _aabb.v[4] = pos.v[2] - zrange; // _aabb.v[5] = pos.v[2] + zrange; _aabb.setMinMax(xrange, yrange, zrange); _aabb.shiftPos(pos); }
private static void computeMassParams (DMass m, DVector3[] q, DVectorN pm) { double pmi, q0, q1, q2; m.setZero (); DVector3 C = m.getC().clone(); DMatrix3 I = m.getI().clone(); for (int i=0; i<NUM; i++) { pmi = pm.get(i); m.setMass(m.getMass() + pmi);// += pmi; C.eqSum( C, q[i], pmi); q0 = q[i].get0(); q1 = q[i].get1(); q2 = q[i].get2(); I.add(0,0, pmi*(q1*q1 + q2*q2)); I.add(1,1, pmi*(q0*q0 + q2*q2)); I.add(2,2, pmi*(q0*q0 + q1*q1)); I.sub(0,1, pmi*(q0*q1)); I.sub(0,2, pmi*(q0*q2)); I.sub(1,2, pmi*(q1*q2)); } //for (j=0; j<3; j++) m.c.v[j] /= m.mass; C.scale(1./m.getMass()); I.set(1,0, I.get(0,1)); I.set(2,0, I.get(0,2)); I.set(2,1, I.get(1,2)); m.setC(C); m.setI(I); }