diff --git a/core/src/main/java/org/ode4j/math/DVector3.java b/core/src/main/java/org/ode4j/math/DVector3.java
index bfb96246..0df5e8c6 100644
--- a/core/src/main/java/org/ode4j/math/DVector3.java
+++ b/core/src/main/java/org/ode4j/math/DVector3.java
@@ -502,43 +502,6 @@ public final DVector3C reAdd(DVector3C c) {
return new DVector3(this).add(c);
}
-
- /**
- * Writes the content of this vector into
- * array at position pos.
- * @param array
- * @param pos
- */
- public final void wrapSet(double[] array, int pos) {
- array[pos] = get0();
- array[pos + 1] = get1();
- array[pos + 2] = get2();
- }
-
- /**
- * Adds the content of this vector to the elements of
- * array at position pos.
- * @param array
- * @param pos
- */
- public final void wrapAdd(double[] array, int pos) {
- array[pos] += get0();
- array[pos + 1] += get1();
- array[pos + 2] += get2();
- }
-
- /**
- * Subtracts the content of this vector from the elements of
- * array at position pos.
- * @param array
- * @param pos
- */
- public final void wrapSub(double[] array, int pos) {
- array[pos] -= get0();
- array[pos + 1] -= get1();
- array[pos + 2] -= get2();
- }
-
/**
* Returns a new vector which equals (this)*d.
* @param d
diff --git a/core/src/main/java/org/ode4j/ode/OdeMath.java b/core/src/main/java/org/ode4j/ode/OdeMath.java
index 6de01a91..b0a24dec 100644
--- a/core/src/main/java/org/ode4j/ode/OdeMath.java
+++ b/core/src/main/java/org/ode4j/ode/OdeMath.java
@@ -130,23 +130,6 @@ public static void dAddScaledVectors3(DVector3 res, DVector3C a, DVector3C b,
// res[1] = -res[1];
// res[2] = -res[2];
// }
- public static void dNegateVector3(double[] a, int ofs) {
- a[ofs+0] = -a[ofs+0];
- a[ofs+1] = -a[ofs+1];
- a[ofs+2] = -a[ofs+2];
- }
-
- public static void dCopyVector3(double[] a, int ofs, final DVector3C b) {
- a[0+ofs] = b.get0();
- a[1+ofs] = b.get1();
- a[2+ofs] = b.get2();
-// dReal res_0, res_1, res_2;
-// res_0 = a[0];
-// res_1 = a[1];
-// res_2 = a[2];
-// // Only assign after all the calculations are over to avoid incurring memory aliasing
-// res[0] = res_0; res[1] = res_1; res[2] = res_2;
- }
public static void dCopyVector3(float[] a, int ofs, final DVector3C b) {
a[0+ofs] = (float) b.get0();
a[1+ofs] = (float) b.get1();
@@ -163,18 +146,6 @@ public static void dCopyVector3(float[] a, int ofs, final DVector3C b) {
// res[0] = res_0; res[1] = res_1; res[2] = res_2;
// }
- public static void dCopyNegatedVector3(double[] a, int ofs, DVector3C b) {
- a[0+ofs] = -b.get0();
- a[1+ofs] = -b.get1();
- a[2+ofs] = -b.get2();
-// dReal res_0, res_1, res_2;
-// res_0 = -a[0];
-// res_1 = -a[1];
-// res_2 = -a[2];
-// // Only assign after all the calculations are over to avoid incurring memory aliasing
-// res[0] = res_0; res[1] = res_1; res[2] = res_2;
- }
-
// PURE_INLINE void dCopyVector4(dReal *res, const dReal *a)
// {
// dReal res_0, res_1, res_2, res_3;
@@ -576,16 +547,6 @@ public static void dSubtractVectorCross3(DVector3 a, DVector3C b, DVector3C c) {
// throw new UnsupportedOperationException(op.name());
// }
// }
- public static void dCalcVectorCross3(double[] a, int ofs, DVector3C b, DVector3C c) {
- a[0+ofs] = (b.get1()*c.get2() - b.get2()*c.get1());
- a[1+ofs] = (b.get2()*c.get0() - b.get0()*c.get2());
- a[2+ofs] = (b.get0()*c.get1() - b.get1()*c.get0());
- }
- public static void dCalcVectorCross3(double[] a, int ofs1, DVector3C b, double[] c, int ofs2) {
- a[0+ofs1] = (b.get1()*c[2+ofs1] - b.get2()*c[1+ofs1]);
- a[1+ofs1] = (b.get2()*c[0+ofs1] - b.get0()*c[2+ofs1]);
- a[2+ofs1] = (b.get0()*c[1+ofs1] - b.get1()*c[0+ofs1]);
- }
// public static void dSubtractVectorCross3(double[] a, int ofs, DVector3C b, DVector3C c) {
// a[0+ofs] = -(b.get1()*c.get2() - b.get2()*c.get1());
// a[1+ofs] = -(b.get2()*c.get0() - b.get0()*c.get2());
@@ -689,20 +650,6 @@ public static void dSetCrossMatrixPlus(double[] A, int ofs, DVector3C a, int ski
A[ofs+2*skip+0] = -a.get1();
A[ofs+2*skip+1] = +a.get0();
}
- /**
- * Set a 3x3 submatrix of A to a matrix such that submatrix(A)*b = a x b.
- * A is stored by rows, and has `skip' elements per row. the matrix is
- * assumed to be already zero, so this does not write zero elements!
- * A negative version will be written.
- */
- public static void dSetCrossMatrixMinus(double[] A, int ofs, DVector3C a, int skip) {
- A[ofs+1] = +a.get2();
- A[ofs+2] = -a.get1();
- A[ofs+skip+0] = -a.get2();
- A[ofs+skip+2] = +a.get0();
- A[ofs+2*skip+0] = +a.get1();
- A[ofs+2*skip+1] = -a.get0();
- }
//#define dCROSSMAT(A,a,skip,plus,minus) \
//do { \
@@ -725,19 +672,6 @@ public static void dCROSSMAT(DMatrix3 A, DVector3C a, int skip, int plus, int mi
A.set20( minus * a.get1() );
A.set21( plus * a.get0() );
}
- /**
- * For +1/-1 use dSetCrossMatrixPlus(), for -1/+1 use dSetCrossMatrixMinus().
- * @deprecated
- */
- public static void dCROSSMAT(double[] A, int ofs, DVector3C a, int skip, int plus, int minus) {
- A[ofs+1] = minus * a.get2();
- A[ofs+2] = plus * a.get1();
- A[ofs+skip+0] = plus * a.get2();
- A[ofs+skip+2] = minus * a.get0();
- A[ofs+2*skip+0] = minus * a.get1();
- A[ofs+2*skip+1] = plus * a.get0();
- }
-
//#ifdef __cplusplus
@@ -840,14 +774,6 @@ private static void dMULTIPLYOP1_331(DVector3 A, DMatrix3C B, DVector3C C) {
A.set1( B.dotCol(1, C) );
A.set2( B.dotCol(2, C) );
}
- private static void dMULTIPLYOP1_331(double[] Aa, int aPos, DMatrix3C B, DVector3C C) {
-// A.set0( dDOT41(B.v, 0, C) );
-// A.set1( dDOT41(B.v, 1, C) );
-// A.set2( dDOT41(B.v, 2, C) );
- Aa[aPos + 0] = B.dotCol(0, C);
- Aa[aPos + 1] = B.dotCol(1, C);
- Aa[aPos + 2] = B.dotCol(2, C);
- }
private static void dMULTIPLYOP0_133(DVector3 A, DVector3C B, DMatrix3C C) {
// A.set0( dDOT14(B, C.v,0) );
// A.set1( dDOT14(B, C.v,1) );
@@ -984,21 +910,9 @@ public static void dMULTIPLY0_331(DMatrix3 A, DMatrix3C B, DVector3C C) {
public static void dMULTIPLY0_331(DVector3 A, DMatrix3C B, double[] C, int c) {
dMultiply0_331(A, B, C, c);
}
- @Deprecated
- public static void dMULTIPLY0_331(double[] A, int a, double[] B, int b,
- DVector3C C) {
- dMultiply0_331(A, a, B, b, C);
- }
- @Deprecated
- public static void dMULTIPLY0_331(double[] A, int a, double[] B, int b,
- double[] C, int c) {
- dMultiply0_331(A, a, B, b, C, c);
- }
public static void dMultiply1_331(DVector3 A, DMatrix3C B, DVector3C C) {
dMULTIPLYOP1_331(A,B,C); }
- public static void dMultiply1_331(double[] A, int a, DMatrix3C B, DVector3C C) {
- dMULTIPLYOP1_331(A,a,B,C); }
public static void dMultiply0_133(DVector3 A, DVector3C B, DMatrix3C C) {
dMULTIPLYOP0_133(A,B,C); }
public static void dMultiply0_133(double[] A, int a, double[] B, int b,
@@ -1020,18 +934,9 @@ public static void dMULTIPLY0_133(DVector3 A, DVector3C B, DMatrix3C C) {
dMultiply0_133(A, B, C);
}
@Deprecated
- public static void dMULTIPLY0_133(double[] A, int a, double[] B, int b,
- double[] C, int c) {
- dMultiply0_133(A, a, B, b, C, c);
- }
- @Deprecated
public static void dMULTIPLY0_333(DMatrix3 A, DMatrix3C B, DMatrix3C C) {
dMultiply0_333(A, B, C);
}
- @Deprecated
- public static void dMULTIPLY0_333(double[] A, int a, DMatrix3C B, DMatrix3C C) {
- dMultiply0_333(A, a, B, C);
- }
// public static void dMULTIPLY0_333(double[] A, int a, double[] B, int b,
// double[] C, int c) {
diff --git a/core/src/main/java/org/ode4j/ode/internal/joints/DxJoint.java b/core/src/main/java/org/ode4j/ode/internal/joints/DxJoint.java
index 594c061d..a6d6acab 100644
--- a/core/src/main/java/org/ode4j/ode/internal/joints/DxJoint.java
+++ b/core/src/main/java/org/ode4j/ode/internal/joints/DxJoint.java
@@ -28,11 +28,8 @@
import static org.ode4j.ode.OdeMath.dCalcVectorDot3;
import static org.ode4j.ode.OdeMath.dMultiply0_331;
import static org.ode4j.ode.OdeMath.dMultiply1_331;
-import static org.ode4j.ode.OdeMath.dNegateVector3;
import static org.ode4j.ode.OdeMath.dNormalize3;
import static org.ode4j.ode.OdeMath.dPlaneSpace;
-import static org.ode4j.ode.OdeMath.dSetCrossMatrixMinus;
-import static org.ode4j.ode.OdeMath.dSetCrossMatrixPlus;
import static org.ode4j.ode.internal.Common.M_PI;
import static org.ode4j.ode.internal.Common.dAtan2;
import static org.ode4j.ode.internal.Common.dIASSERT;
@@ -152,8 +149,7 @@ public static class Info2Descr
private int _rowskip;
public int rowskip() { return _rowskip; }
public void setRowskip(int rs) {_rowskip = rs;}
- double[] _J;
- public double[] getJ() { return _J; }
+ private double[] _J;
public void setJ(double[] J) {_J = J;}
// right hand sides of the equation J*v = c + cfm * lambda. cfm is the
@@ -161,9 +157,9 @@ public static class Info2Descr
// set to a constant value (typically very small or zero) value on entry.
// public dVector3 c = new dVector3();//double[] c; //TZ: [4] ? 0,1,2,3
// public dVector3 cfm = new dVector3(); //TZ: [3] ?
- protected double[] _cA;
+ private double[] _cA;
int _cP;
- protected double[] _cfmA;
+ private double[] _cfmA;
int _cfmP;
public void setC(int i, double d) {
_cA[_cP+i] = d;
@@ -230,7 +226,7 @@ public void setArrays(double[] J, double[] c, double[] cfm,
}
@Override
public String toString() {
- System.out.println("rowskip : " + rowskip());
+ System.out.println("rowskip : " + _rowskip);
System.out.println("Jxxp : " + J1lp + "/" + J1ap + " / " + J2lp + "/" + J2ap);
for (int i = 0; i < _J.length; i+=8) {
System.out.println("_J : " +
@@ -244,6 +240,65 @@ public String toString() {
System.out.println("_findexA: " + Arrays.toString(_findexA));
return super.toString();
}
+
+ public void setJ1l(int row, int i, double d) {
+ _J[J1lp + row * _rowskip + i] = d;
+ }
+ public void setJ1a(int row, int i, double d) {
+ _J[J1ap + row * _rowskip + i] = d;
+ }
+ public void setJ2l(int row, int i, double d) {
+ _J[J2lp + row * _rowskip + i] = d;
+ }
+ public void setJ2a(int row, int i, double d) {
+ _J[J2ap + row * _rowskip + i] = d;
+ }
+ public void setJ1l(int row, DVector3C v) {
+ setJ1l(row, 0, v.get0());
+ setJ1l(row, 1, v.get1());
+ setJ1l(row, 2, v.get2());
+ }
+ public void setJ1a(int row, DVector3C v) {
+ setJ1a(row, 0, v.get0());
+ setJ1a(row, 1, v.get1());
+ setJ1a(row, 2, v.get2());
+ }
+ public void setJ2l(int row, DVector3C v) {
+ setJ2l(row, 0, v.get0());
+ setJ2l(row, 1, v.get1());
+ setJ2l(row, 2, v.get2());
+ }
+ public void setJ2a(int row, DVector3C v) {
+ setJ2a(row, 0, v.get0());
+ setJ2a(row, 1, v.get1());
+ setJ2a(row, 2, v.get2());
+ }
+ public void setJ2lNegated(int row, DVector3C v) {
+ setJ2l(row, 0, -v.get0());
+ setJ2l(row, 1, -v.get1());
+ setJ2l(row, 2, -v.get2());
+ }
+ public void setJ2aNegated(int row, DVector3C v) {
+ setJ2a(row, 0, -v.get0());
+ setJ2a(row, 1, -v.get1());
+ setJ2a(row, 2, -v.get2());
+ }
+ public void setJ1aCrossMatrix(int row, DVector3C a, double sign) {
+ setJ1a(row, 1, -sign * a.get2());
+ setJ1a(row, 2, sign * a.get1());
+ setJ1a(row + 1, 0, sign * a.get2());
+ setJ1a(row + 1, 2, -sign * a.get0());
+ setJ1a(row + 2, 0, -sign * a.get1());
+ setJ1a(row + 2, 1, sign * a.get0());
+ }
+ public void setJ2aCrossMatrix(int row, DVector3C a, double sign) {
+ setJ2a(row, 1, -sign * a.get2());
+ setJ2a(row, 2, sign * a.get1());
+ setJ2a(row + 1, 0, sign * a.get2());
+ setJ2a(row + 1, 2, -sign * a.get0());
+ setJ2a(row + 2, 0, -sign * a.get1());
+ setJ2a(row + 2, 1, sign * a.get0());
+ }
}
// info returned by getSureMaxInfo function.
@@ -404,24 +459,22 @@ void setBall( DxJoint joint, double fps, double erp, Info2Descr info,
// anchor points in global coordinates with respect to body PORs.
DVector3 a1 = new DVector3(), a2 = new DVector3();
- int s = info.rowskip();
-
// set jacobian
- info._J[info.J1lp+0] = 1;
- info._J[info.J1lp+s+1] = 1;
- info._J[info.J1lp+2*s+2] = 1;
+ info.setJ1l(0, 0, 1);
+ info.setJ1l(1, 1, 1);
+ info.setJ1l(2, 2, 1);
dMultiply0_331( a1, joint.node[0].body.posr().R(), anchor1 );
// dCROSSMAT( info.J1a, a1, s, -, + );
- dSetCrossMatrixMinus( info._J, info.J1ap, a1, s );
+ info.setJ1aCrossMatrix(0, a1, -1);
DxBody b1 = joint.node[1].body;
if ( b1 != null)
{
- info._J[info.J2lp+0] = -1;
- info._J[info.J2lp+s+1] = -1;
- info._J[info.J2lp+2*s+2] = -1;
+ info.setJ2l(0, 0, -1);
+ info.setJ2l(1, 1, -1);
+ info.setJ2l(2, 2, -1);
dMultiply0_331( a2, b1.posr().R(), anchor2 );
- dSetCrossMatrixPlus( info._J, info.J2ap, a2, s );
+ info.setJ2aCrossMatrix(0, a2, 1);
}
@@ -471,8 +524,6 @@ void setBall2( DxJoint joint, double fps, double erp, Info2Descr info,
// anchor points in global coordinates with respect to body PORs.
DVector3 a1 = new DVector3(), a2 = new DVector3();
- int s = info.rowskip();
-
// get vectors normal to the axis. in setBall() axis,q1,q2 is [1 0 0],
// [0 1 0] and [0 0 1], which makes everything much easier.
DVector3 q1 = new DVector3(), q2 = new DVector3();
@@ -482,13 +533,17 @@ void setBall2( DxJoint joint, double fps, double erp, Info2Descr info,
// for ( i = 0; i < 3; i++ ) info._J[info.J1lp+i] = axis.get(i);
// for ( i = 0; i < 3; i++ ) info._J[info.J1lp+s+i] = q1.get(i);
// for ( i = 0; i < 3; i++ ) info._J[info.J1lp+2*s+i] = q2.get(i);
- axis.wrapSet( info._J, info.J1lp );
- q1.wrapSet( info._J, info.J1lp+s );
- q2.wrapSet( info._J, info.J1lp+2*s );
+ info.setJ1l(0, axis);
+ info.setJ1l(1, q1);
+ info.setJ1l(2, q2);
dMultiply0_331( a1, joint.node[0].body.posr().R(), anchor1 );
- dCalcVectorCross3( info._J, info.J1ap, a1, axis );
- dCalcVectorCross3( info._J, info.J1ap + s, a1, q1 );
- dCalcVectorCross3( info._J, info.J1ap + 2*s, a1, q2 );
+ DVector3 v = new DVector3();
+ dCalcVectorCross3( v, a1, axis );
+ info.setJ1a(0, v);
+ dCalcVectorCross3( v, a1, q1 );
+ info.setJ1a(1, v);
+ dCalcVectorCross3( v, a1, q2 );
+ info.setJ1a(2, v);
DxBody b1 = joint.node[1].body;
if ( b1 != null)
@@ -496,9 +551,9 @@ void setBall2( DxJoint joint, double fps, double erp, Info2Descr info,
// for ( i = 0; i < 3; i++ ) info._J[info.J2lp+i] = -axis.v[i];
// for ( i = 0; i < 3; i++ ) info._J[info.J2lp+s+i] = -q1.v[i];
// for ( i = 0; i < 3; i++ ) info._J[info.J2lp+2*s+i] = -q2.v[i];
- axis.wrapSub( info._J, info.J2lp );
- q1.wrapSub( info._J, info.J2lp+s );
- q2.wrapSub( info._J, info.J2lp+2*s );
+ info.setJ2lNegated(0, axis);
+ info.setJ2lNegated(1, q1);
+ info.setJ2lNegated(2, q2);
// dMultiply0_331( a2, joint->node[1].body->posr.R, anchor2 );
// dReal *J2a = info->J2a;
// dCalcVectorCross3( J2a, a2, axis );
@@ -510,17 +565,15 @@ void setBall2( DxJoint joint, double fps, double erp, Info2Descr info,
// dCalcVectorCross3( J2a_plus_2s, a2, q2 );
// dNegateVector3( J2a_plus_2s );
dMultiply0_331( a2, b1._posr.R(), anchor2 );
- double[] J = info._J; //TZ
- int J2ap = info.J2ap; //TZ
//dReal *J2a = info->J2a;
- dCalcVectorCross3( J, J2ap, a2, axis );
- dNegateVector3( J, J2ap );
+ dCalcVectorCross3( v, a2, axis );
+ info.setJ2aNegated(0, v);
//dReal *J2a_plus_s = J2a + s;
- dCalcVectorCross3( J, J2ap+s, a2, q1 );
- dNegateVector3( J, J2ap+s );
+ dCalcVectorCross3( v, a2, q1 );
+ info.setJ2aNegated(1, v);
//dReal *J2a_plus_2s = J2a_plus_s + s;
- dCalcVectorCross3( J, J2ap+2*s, a2, q2 );
- dNegateVector3( J, J2ap+2*s );
+ dCalcVectorCross3( v, a2, q2 );
+ info.setJ2aNegated(2, v);
}
// set right hand side - measure error along (axis,q1,q2)
@@ -562,19 +615,17 @@ void setBall2( DxJoint joint, double fps, double erp, Info2Descr info,
void setFixedOrientation( DxJoint joint, double fps, double erp, Info2Descr info,
DQuaternion qrel, int start_row )
{
- int s = info.rowskip();
- int start_index = start_row * s;
// 3 rows to make body rotations equal
- info._J[info.J1ap+start_index] = 1;
- info._J[info.J1ap+start_index + s + 1] = 1;
- info._J[info.J1ap+start_index + s*2+2] = 1;
+ info.setJ1a(start_row, 0, 1);
+ info.setJ1a(start_row + 1, 1, 1);
+ info.setJ1a(start_row + 2, 2, 1);
DxBody b1 = joint.node[1].body;
if ( b1 != null)
{
- info._J[info.J2ap+start_index] = -1;
- info._J[info.J2ap+start_index + s+1] = -1;
- info._J[info.J2ap+start_index + s*2+2] = -1;
+ info.setJ2a(start_row, 0, -1);
+ info.setJ2a(start_row + 1, 1, -1);
+ info.setJ2a(start_row + 2, 2, -1);
}
// compute the right hand side. the first three elements will result in
diff --git a/core/src/main/java/org/ode4j/ode/internal/joints/DxJointContact.java b/core/src/main/java/org/ode4j/ode/internal/joints/DxJointContact.java
index 9f81edb5..ec6f90a7 100644
--- a/core/src/main/java/org/ode4j/ode/internal/joints/DxJointContact.java
+++ b/core/src/main/java/org/ode4j/ode/internal/joints/DxJointContact.java
@@ -41,13 +41,11 @@
import static org.ode4j.ode.OdeMath.dCalcVectorCross3;
import static org.ode4j.ode.OdeMath.dCalcVectorDot3;
import static org.ode4j.ode.OdeMath.dPlaneSpace;
-import static org.ode4j.ode.OdeMath.dNegateVector3;
import org.ode4j.math.DVector3;
import org.ode4j.ode.DContact;
import org.ode4j.ode.DContactJoint;
import org.ode4j.ode.OdeConstants;
-import org.ode4j.ode.OdeMath;
import org.ode4j.ode.internal.DxBody;
import org.ode4j.ode.internal.DxWorld;
@@ -129,8 +127,6 @@ void getSureMaxInfo( SureMaxInfo info )
public void
getInfo2( double worldFPS, double worldERP, DxJoint.Info2Descr info )
{
- int s = info.rowskip();
- int s2 = 2 * s;
final int rowNormal = 0;
final int rowFriction1 = 1;
@@ -162,24 +158,28 @@ void getSureMaxInfo( SureMaxInfo info )
// c1.v[2] = contact.geom.pos.v[2] - node[0].body._posr.pos.v[2];
c1.eqDiff( contact.geom.pos, node[0].body.posr().pos() );
+ DVector3 j1l = new DVector3();
+ DVector3 j1a = new DVector3();
// set jacobian for normal
- info._J[info.J1lp+0] = normal.get0();
- info._J[info.J1lp+1] = normal.get1();
- info._J[info.J1lp+2] = normal.get2();
- dCalcVectorCross3( info._J, info.J1ap, c1, normal );
+ info.setJ1l(0, normal);
+ j1l.set(normal);
+ dCalcVectorCross3( j1a, c1, normal );
+ info.setJ1a(0, j1a);
DxBody b1 = node[1].body;
+ DVector3 j2l = new DVector3();
+ DVector3 j2a = new DVector3();
if ( b1 != null)
{
// c2.v[0] = contact.geom.pos.v[0] - node[1].body._posr.pos.v[0];
// c2.v[1] = contact.geom.pos.v[1] - node[1].body._posr.pos.v[1];
// c2.v[2] = contact.geom.pos.v[2] - node[1].body._posr.pos.v[2];
c2.eqDiff( contact.geom.pos, b1.posr().pos() );
- info._J[info.J2lp+0] = -normal.get0();
- info._J[info.J2lp+1] = -normal.get1();
- info._J[info.J2lp+2] = -normal.get2();
- dCalcVectorCross3( info._J, info.J2ap, c2, normal );
- dNegateVector3( info._J, info.J2ap );
+ j2l.set(normal).scale(-1);
+ info.setJ2l(0, j2l);
+ dCalcVectorCross3( j2a, c2, normal );
+ j2a.scale(-1);
+ info.setJ2a(0, j2a);
}
// set right hand side and cfm value for normal
@@ -211,13 +211,13 @@ void getSureMaxInfo( SureMaxInfo info )
{
// calculate outgoing velocity (-ve for incoming contact)
double outgoing =
- dCalcVectorDot3( info._J, info.J1lp, node[0].body.lvel )
- + dCalcVectorDot3( info._J, info.J1ap, node[0].body.avel );
+ dCalcVectorDot3( j1l, node[0].body.lvel )
+ + dCalcVectorDot3( j1a, node[0].body.avel );
if ( b1 != null)
{
outgoing +=
- dCalcVectorDot3( info._J, info.J2lp, node[1].body.lvel )
- + dCalcVectorDot3( info._J, info.J2ap, node[1].body.avel );
+ dCalcVectorDot3( j2l, node[1].body.lvel )
+ + dCalcVectorDot3( j2a, node[1].body.avel );
}
outgoing -= motionN;
// only apply bounce if the outgoing velocity is greater than the
@@ -256,21 +256,20 @@ void getSureMaxInfo( SureMaxInfo info )
// first friction direction
if ( contact.surface.mu > 0 )
{
- info._J[info.J1lp+s+0] = t1.get0();
- info._J[info.J1lp+s+1] = t1.get1();
- info._J[info.J1lp+s+2] = t1.get2();
- dCalcVectorCross3( info._J, info.J1ap + s, c1, t1 );
+ info.setJ1l(1, t1);
+ DVector3 j1a_1 = new DVector3();
+ dCalcVectorCross3( j1a_1, c1, t1 );
+ info.setJ1a(1, j1a_1);
if ( node[1].body != null)
{
- info._J[info.J2lp+s+0] = -t1.get0();
- info._J[info.J2lp+s+1] = -t1.get1();
- info._J[info.J2lp+s+2] = -t1.get2();
+ DVector3 j2a_1 = new DVector3();
+ info.setJ2lNegated(1, t1);
// dReal *J2a_plus_s = info->J2a + s;
// dCalcVectorCross3( J2a_plus_s, c2, t1 );
// dNegateVector3( J2a_plus_s );
- dCalcVectorCross3( info._J, info.J2ap+s, c2, t1 );
- dNegateVector3( info._J, info.J2ap+s );
+ dCalcVectorCross3( j2a_1, c2, t1 );
+ info.setJ2aNegated(1, j2a_1);
}
// set right hand side
@@ -292,7 +291,6 @@ void getSureMaxInfo( SureMaxInfo info )
} else {
// there was no friction for direction 1, so the second friction constraint
// has to be on this line instead
- s2 = s;
rowFriction2 = rowFriction1;
}
@@ -301,21 +299,20 @@ void getSureMaxInfo( SureMaxInfo info )
// second friction direction
if ( mu2 > 0 )
{
- info._J[info.J1lp+s2+0] = t2.get0();
- info._J[info.J1lp+s2+1] = t2.get1();
- info._J[info.J1lp+s2+2] = t2.get2();
- dCalcVectorCross3( info._J, info.J1ap + s2, c1, t2 );
+ info.setJ1l(2, t2);
+ DVector3 j1a_2 = new DVector3();
+ dCalcVectorCross3( j1a_2, c1, t2 );
+ info.setJ1a(2, j1a_2);
if ( node[1].body != null)
{
- info._J[info.J2lp+s2+0] = -t2.get0();
- info._J[info.J2lp+s2+1] = -t2.get1();
- info._J[info.J2lp+s2+2] = -t2.get2();
+ info.setJ2lNegated(2, t2);
// dReal *J2a_plus_s2 = info->J2a + s2;
// dCalcVectorCross3( J2a_plus_s2, c2, t2 );
// dNegateVector3( J2a_plus_s2 );
- dCalcVectorCross3( info._J, info.J2ap+s2, c2, t2 );
- dNegateVector3( info._J, info.J2ap+s2 );
+ DVector3 j2a_2 = new DVector3();
+ dCalcVectorCross3( j2a_2, c2, t2 );
+ info.setJ2aNegated(2, j2a_2);
}
// set right hand side
@@ -366,9 +363,9 @@ void getSureMaxInfo( SureMaxInfo info )
for (int ii=0;ii<3;++ii) {
if (rho[ii]>0) {
// Set the angular axis
- OdeMath.dCopyVector3(info._J, info.J1ap+rollRow*s, ax[ii]);
+ info.setJ1a(rollRow, ax[ii]);
if ( b1!=null ) {
- OdeMath.dCopyNegatedVector3(info._J, info.J2ap+rollRow*s, ax[ii]);
+ info.setJ2aNegated(rollRow, ax[ii]);
}
// Set the lcp limits
info.setLo( rollRow, -rho[ii]);
diff --git a/core/src/main/java/org/ode4j/ode/internal/joints/DxJointDBall.java b/core/src/main/java/org/ode4j/ode/internal/joints/DxJointDBall.java
index 029df0e5..c083deff 100644
--- a/core/src/main/java/org/ode4j/ode/internal/joints/DxJointDBall.java
+++ b/core/src/main/java/org/ode4j/ode/internal/joints/DxJointDBall.java
@@ -72,7 +72,7 @@ public class DxJointDBall extends DxJoint implements DDoubleBallJoint {
public void
getInfo2( double worldFPS, double worldERP, Info2Descr info )
{
- info._cfmA[0] = this.cfm;
+ info.setCfm(0, this.cfm);
DVector3 globalA1 = new DVector3(), globalA2 = new DVector3();
//dBodyGetRelPointPos(node[0].body, anchor1[0], anchor1[1], anchor1[2], globalA1);
@@ -120,9 +120,7 @@ public class DxJointDBall extends DxJoint implements DDoubleBallJoint {
}
OdeMath.dNormalize3(q);
- info._J[info.J1lp+0] = q.get0();
- info._J[info.J1lp+1] = q.get1();
- info._J[info.J1lp+2] = q.get2();
+ info.setJ1l(0, q);
DVector3 relA1 = new DVector3();
// dBodyVectorToWorld(node[0].body,
@@ -133,13 +131,12 @@ public class DxJointDBall extends DxJoint implements DDoubleBallJoint {
DMatrix3 a1m = new DMatrix3();
//dSetZero(a1m, 12);
dSetCrossMatrixMinus(a1m, relA1);//, 4);
-
- dMultiply1_331(info._J,info.J1ap, a1m, q);
+ DVector3 v = new DVector3();
+ dMultiply1_331(v, a1m, q);
+ info.setJ1a(0, v);
if (node[1].body != null) {
- info._J[info.J2lp+0] = -q.get0();
- info._J[info.J2lp+1] = -q.get1();
- info._J[info.J2lp+2] = -q.get2();
+ info.setJ2lNegated(0, q);
DVector3 relA2 = new DVector3();
// dBodyVectorToWorld(node[1].body,
@@ -149,7 +146,8 @@ public class DxJointDBall extends DxJoint implements DDoubleBallJoint {
DMatrix3 a2m = new DMatrix3();
//dSetZero(a2m, 12);
dSetCrossMatrixPlus(a2m, relA2);//, 4);
- dMultiply1_331(info._J,info.J2ap, a2m, q);
+ dMultiply1_331(v, a2m, q);
+ info.setJ2a(0, v);
}
final double k = worldFPS * this.erp;
diff --git a/core/src/main/java/org/ode4j/ode/internal/joints/DxJointDHinge.java b/core/src/main/java/org/ode4j/ode/internal/joints/DxJointDHinge.java
index 6a7ede88..79fee0c5 100644
--- a/core/src/main/java/org/ode4j/ode/internal/joints/DxJointDHinge.java
+++ b/core/src/main/java/org/ode4j/ode/internal/joints/DxJointDHinge.java
@@ -78,11 +78,6 @@ public class DxJointDHinge extends DxJointDBall implements DDoubleHingeJoint {
{
super.getInfo2( worldFPS, worldERP, info ); // sets row0
- final int skip = info.rowskip();
- final int row1 = skip;
- final int row2 = 2*skip;
- final int row3 = 3*skip;
-
DVector3 globalAxis1 = new DVector3();
//dBodyVectorToWorld(node[0].body, axis1[0], axis1[1], axis1[2], globalAxis1);
node[0].body.vectorToWorld(axis1, globalAxis1);
@@ -90,20 +85,12 @@ public class DxJointDHinge extends DxJointDBall implements DDoubleHingeJoint {
// angular constraints, perpendicular to axis
DVector3 p = new DVector3(), q = new DVector3();
dPlaneSpace(globalAxis1, p, q);
- info._J[info.J1ap+row1+0] = p.get0();
- info._J[info.J1ap+row1+1] = p.get1();
- info._J[info.J1ap+row1+2] = p.get2();
- info._J[info.J1ap+row2+0] = q.get0();
- info._J[info.J1ap+row2+1] = q.get1();
- info._J[info.J1ap+row2+2] = q.get2();
+ info.setJ1a(1, p);
+ info.setJ1a(2, q);
if ( node[1].body != null ) {
- info._J[info.J2ap+row1+0] = -p.get0();
- info._J[info.J2ap+row1+1] = -p.get1();
- info._J[info.J2ap+row1+2] = -p.get2();
- info._J[info.J2ap+row2+0] = -q.get0();
- info._J[info.J2ap+row2+1] = -q.get1();
- info._J[info.J2ap+row2+2] = -q.get2();
+ info.setJ2aNegated(1, p);
+ info.setJ2aNegated(2, q);
}
DVector3 globalAxis2 = new DVector3();
@@ -144,7 +131,7 @@ public class DxJointDHinge extends DxJointDBall implements DDoubleHingeJoint {
//info.J1l[row3+0] = globalAxis1[0];
//info.J1l[row3+1] = globalAxis1[1];
//info.J1l[row3+2] = globalAxis1[2];
- globalAxis1.wrapSet(info._J, info.J1lp+row3);
+ info.setJ1l(3, globalAxis1);
if ( node[1].body != null ) {
@@ -157,17 +144,17 @@ public class DxJointDHinge extends DxJointDBall implements DDoubleHingeJoint {
//info.J1a[row3+0] = omega[0];
//info.J1a[row3+1] = omega[1];
//info.J1a[row3+2] = omega[2];
- omega.wrapSet(info._J, info.J1ap+row3);
+ info.setJ1a(3, omega);
//info.J2l[row3+0] = -globalAxis1[0];
//info.J2l[row3+1] = -globalAxis1[1];
//info.J2l[row3+2] = -globalAxis1[2];
- globalAxis1.wrapSet(info._J, info.J2lp+row3);
+ info.setJ2lNegated(3, globalAxis1);
//info.J2a[row3+0] = omega[0];
//info.J2a[row3+1] = omega[1];
//info.J2a[row3+2] = omega[2];
- omega.wrapSet(info._J, info.J2ap+row3);
+ info.setJ2a(3, omega);
}
// error correction: both anchors should lie on the same plane perpendicular to the axis
diff --git a/core/src/main/java/org/ode4j/ode/internal/joints/DxJointFixed.java b/core/src/main/java/org/ode4j/ode/internal/joints/DxJointFixed.java
index 0d091c5b..63e1fbe2 100644
--- a/core/src/main/java/org/ode4j/ode/internal/joints/DxJointFixed.java
+++ b/core/src/main/java/org/ode4j/ode/internal/joints/DxJointFixed.java
@@ -90,16 +90,15 @@ public void getInfo1 ( Info1 info )
@Override
public void getInfo2 ( double worldFPS, double worldERP, Info2Descr info )
{
- int s = info.rowskip();
// Three rows for orientation
setFixedOrientation ( this, worldFPS, worldERP, info, qrel, 3 );
// Three rows for position.
// set jacobian
- info._J[info.J1lp+0] = 1;
- info._J[info.J1lp+s+1] = 1;
- info._J[info.J1lp+2*s+2] = 1;
+ info.setJ1l(0, 0, 1);
+ info.setJ1l(1, 1, 1);
+ info.setJ1l(2, 2, 1);
info.setCfm(0, cfm);
info.setCfm(1, cfm);
@@ -111,10 +110,10 @@ public void getInfo2 ( double worldFPS, double worldERP, Info2Descr info )
dMultiply0_331 ( ofs, b0.posr().R(), offset );
if ( b1 != null )
{
- dSetCrossMatrixPlus ( info._J, info.J1ap, ofs, s);
- info._J[info.J2lp+0] = -1;
- info._J[info.J2lp+s+1] = -1;
- info._J[info.J2lp+2*s+2] = -1;
+ info.setJ1aCrossMatrix(0, ofs, 1);
+ info.setJ2l(0, 0, -1);
+ info.setJ2l(1, 1, -1);
+ info.setJ2l(2, 2, -1);
}
// set right hand side for the first three rows (linear)
diff --git a/core/src/main/java/org/ode4j/ode/internal/joints/DxJointHinge.java b/core/src/main/java/org/ode4j/ode/internal/joints/DxJointHinge.java
index bf2909b9..efaa9065 100644
--- a/core/src/main/java/org/ode4j/ode/internal/joints/DxJointHinge.java
+++ b/core/src/main/java/org/ode4j/ode/internal/joints/DxJointHinge.java
@@ -128,28 +128,13 @@ void getSureMaxInfo( SureMaxInfo info )
dMultiply0_331( ax1, node[0].body.posr().R(), _axis1 );
dPlaneSpace( ax1, p, q );
- int s3 = 3 * info.rowskip();
- int s4 = 4 * info.rowskip();
-
-// info._J[info.J1ap+s3+0] = p.v[0];
-// info._J[info.J1ap+s3+1] = p.v[1];
-// info._J[info.J1ap+s3+2] = p.v[2];
- p.wrapSet( info._J, info.J1ap+s3 );
-// info._J[info.J1ap+s4+0] = q.v[0];
-// info._J[info.J1ap+s4+1] = q.v[1];
-// info._J[info.J1ap+s4+2] = q.v[2];
- q.wrapSet( info._J, info.J1ap+s4 );
+ info.setJ1a(3, p);
+ info.setJ1a(4, q);
if ( node[1].body!= null )
{
-// info._J[info.J2ap+s3+0] = -p.v[0];
-// info._J[info.J2ap+s3+1] = -p.v[1];
-// info._J[info.J2ap+s3+2] = -p.v[2];
- p.wrapSub( info._J, info.J2ap+s3 );
-// info._J[info.J2ap+s4+0] = -q.v[0];
-// info._J[info.J2ap+s4+1] = -q.v[1];
-// info._J[info.J2ap+s4+2] = -q.v[2];
- q.wrapSub( info._J, info.J2ap+s4 );
+ info.setJ2aNegated(3, p);
+ info.setJ2aNegated(4, q);
}
// compute the right hand side of the constraint equation. set relative
diff --git a/core/src/main/java/org/ode4j/ode/internal/joints/DxJointHinge2.java b/core/src/main/java/org/ode4j/ode/internal/joints/DxJointHinge2.java
index fe2cdfbb..91a31cd7 100644
--- a/core/src/main/java/org/ode4j/ode/internal/joints/DxJointHinge2.java
+++ b/core/src/main/java/org/ode4j/ode/internal/joints/DxJointHinge2.java
@@ -195,17 +195,16 @@ private void getAxisInfo(DVector3 ax1, DVector3 ax2, DVector3 axCross,
setBall2( this, worldFPS, worldERP, info, anchor1, anchor2, ax1, susp_erp );
// set the hinge row
- int s3 = 3 * info.rowskip();
// info._J[info.J1ap+s3+0] = q.v[0];
// info._J[info.J1ap+s3+1] = q.v[1];
// info._J[info.J1ap+s3+2] = q.v[2];
- q.wrapSet( info._J, info.J1ap+s3 );
+ info.setJ1a(3, q);
if ( joint.node[1].body != null)
{
// info._J[info.J2ap+s3+0] = -q.v[0];
// info._J[info.J2ap+s3+1] = -q.v[1];
// info._J[info.J2ap+s3+2] = -q.v[2];
- q.wrapSub( info._J, info.J2ap+s3 );
+ info.setJ2aNegated(3, q);
}
// compute the right hand side for the constrained rotational DOF.
diff --git a/core/src/main/java/org/ode4j/ode/internal/joints/DxJointLimitMotor.java b/core/src/main/java/org/ode4j/ode/internal/joints/DxJointLimitMotor.java
index 44fe3399..2c280291 100644
--- a/core/src/main/java/org/ode4j/ode/internal/joints/DxJointLimitMotor.java
+++ b/core/src/main/java/org/ode4j/ode/internal/joints/DxJointLimitMotor.java
@@ -169,7 +169,6 @@ public int addLimot( DxJoint joint, double fps,
Info2Descr info, int row,
final DVector3C ax1, boolean rotational )
{
- int srow = row * info.rowskip();
// if the joint is powered, or has joint limits, add in the extra row
boolean powered = fmax > 0;
@@ -189,24 +188,16 @@ public int addLimot( DxJoint joint, double fps,
// }
DxBody b1 = joint.node[1].body;
if (rotational) {
- info._J[info.J1ap+srow+0] = ax1.get0();
- info._J[info.J1ap+srow+1] = ax1.get1();
- info._J[info.J1ap+srow+2] = ax1.get2();
+ info.setJ1a(row, ax1);
if ( b1 != null )
{
- info._J[info.J2ap+srow+0] = -ax1.get0();
- info._J[info.J2ap+srow+1] = -ax1.get1();
- info._J[info.J2ap+srow+2] = -ax1.get2();
+ info.setJ2aNegated(row, ax1);
}
} else {
- info._J[info.J1lp+srow+0] = ax1.get0();
- info._J[info.J1lp+srow+1] = ax1.get1();
- info._J[info.J1lp+srow+2] = ax1.get2();
+ info.setJ1l(row, ax1);
if ( b1 != null )
{
- info._J[info.J2lp+srow+0] = -ax1.get0();
- info._J[info.J2lp+srow+1] = -ax1.get1();
- info._J[info.J2lp+srow+2] = -ax1.get2();
+ info.setJ2lNegated(row, ax1);
}
}
@@ -234,12 +225,8 @@ public int addLimot( DxJoint joint, double fps,
// c.v[2] = 0.5 * ( joint.node[1].body._posr.pos.v[2] - joint.node[0].body._posr.pos.v[2] );
c.eqDiff(b1.posr().pos(), b0.posr().pos()).scale(0.5);
dCalcVectorCross3( ltd, c, ax1 );
- info._J[info.J1ap+srow+0] = ltd.get0();
- info._J[info.J1ap+srow+1] = ltd.get1();
- info._J[info.J1ap+srow+2] = ltd.get2();
- info._J[info.J2ap+srow+0] = ltd.get0();
- info._J[info.J2ap+srow+1] = ltd.get1();
- info._J[info.J2ap+srow+2] = ltd.get2();
+ info.setJ1a(row, ltd);
+ info.setJ2a(row, ltd);
}
// if we're limited low and high simultaneously, the joint motor is
@@ -399,26 +386,33 @@ int addTwoPointLimot( DxJoint joint, double fps,
Info2Descr info, int row,
DVector3C ax1, DVector3C pt1, DVector3C pt2 )
{
- int srow = row * info.rowskip();
// if the joint is powered, or has joint limits, add in the extra row
boolean powered = fmax > 0;
+ DVector3 j1l = new DVector3();
+ DVector3 j1a = new DVector3();
+ DVector3 j2l = new DVector3();
+ DVector3 j2a = new DVector3();
if ( powered || limit != 0)
{
// Set the linear portion
//dCopyVector3((info.J1l[srow]),ax1);
- OdeMath.dCopyVector3(info._J, info.J1lp+srow, ax1);
+ j1l.set(ax1);
+ info.setJ1l(row, j1l);
// Set the angular portion (to move the linear constraint
// away from the center of mass).
//dCalcVectorCross3((info.J1a[srow]),pt1,ax1);
- dCalcVectorCross3(info._J,info.J1ap+srow,pt1,ax1);
+ dCalcVectorCross3(j1a,pt1,ax1);
+ info.setJ1a(row, j1a);
// Set the constraints for the second body
if ( joint.node[1].body != null ) {
- OdeMath.dCopyNegatedVector3(info._J, info.J2lp+srow, ax1);
+ j2l.set(ax1);
+ j2l.scale(-1);
+ info.setJ2l(row, j2l);
//dCalcVectorCross3(&(info->J2a[srow]),pt2,&(info->J2l[srow]));
- OdeMath.dCalcVectorCross3(info._J, info.J2ap+srow, pt2,
- info._J, info.J2lp+srow);
+ OdeMath.dCalcVectorCross3(j2a, pt2, j2l);
+ info.setJ2a(row, j2a);
}
// if we're limited low and high simultaneously, the joint motor is
@@ -460,18 +454,15 @@ int addTwoPointLimot( DxJoint joint, double fps,
DxBody b0 = joint.node[0].body;
b0.dBodyAddForce( -fm*ax1.get0(), -fm*ax1.get1(), -fm*ax1.get2() );
//b0.dBodyAddTorque( -fm*tAx1[0], -fm*tAx1[1], -fm*tAx1[2] );
- double[] J = info.getJ();
- int j1a = info.J1ap + srow;
- b0.dBodyAddTorque( -fm*J[j1a+0], -fm*J[j1a+1], -fm*J[j1a+2] );
+ b0.dBodyAddTorque( -fm*j1a.get0(), -fm*j1a.get1(), -fm*j1a.get2() );
DxBody b1 = joint.node[1].body;
if ( b1 != null )
{
//const dReal* tAx2 = &(info.J2a[srow]);
b1.dBodyAddForce( fm*ax1.get0(), fm*ax1.get1(), fm*ax1.get2() );
- int j2a = info.J2ap + srow;
//b1.dBodyAddTorque( -fm*tAx2[0], -fm*tAx2[1], -fm*tAx2[2] );
- b1.dBodyAddTorque( -fm*J[j2a+0], -fm*J[j2a+1], -fm*J[j2a+2] );
+ b1.dBodyAddTorque( -fm*j2a.get0(), -fm*j2a.get1(), -fm*j2a.get2() );
}
}
@@ -512,15 +503,15 @@ int addTwoPointLimot( DxJoint joint, double fps,
double vel =
// dCalcVectorDot3( joint.node[0].body.lvel, &(info.J1l[srow])) +
// dCalcVectorDot3( joint.node[0].body.avel, &(info.J1a[srow]));
- b0.lvel.dot (info._J, info.J1lp+srow) +
- b0.avel.dot (info._J, info.J1ap+srow);
+ b0.lvel.dot (j1l) +
+ b0.avel.dot (j1a);
DxBody b1 = joint.node[1].body;
if (joint.node[1].body != null) {
vel +=
// dCalcVectorDot3( joint.node[1].body.lvel, &(info.J2l[srow])) +
// dCalcVectorDot3( joint.node[1].body.avel, &(info.J2a[srow]));
- b1.lvel.dot (info._J, info.J2lp+srow) +
- b1.avel.dot (info._J, info.J2ap+srow);
+ b1.lvel.dot (j2l) +
+ b1.avel.dot (j2a);
}
// only apply bounce if the velocity is incoming, and if the
diff --git a/core/src/main/java/org/ode4j/ode/internal/joints/DxJointPR.java b/core/src/main/java/org/ode4j/ode/internal/joints/DxJointPR.java
index 7ed48619..5eb1f00f 100644
--- a/core/src/main/java/org/ode4j/ode/internal/joints/DxJointPR.java
+++ b/core/src/main/java/org/ode4j/ode/internal/joints/DxJointPR.java
@@ -296,10 +296,6 @@ void getSureMaxInfo( SureMaxInfo info )
public void
getInfo2( double worldFPS, double worldERP, DxJoint.Info2Descr info )
{
- int s = info.rowskip();
- int s2 = 2 * s;
- int s3 = 3 * s;
- //int s4= 4*s;
double k = worldFPS * worldERP;
@@ -371,21 +367,13 @@ void getSureMaxInfo( SureMaxInfo info )
dMultiply0_331( ax1, node[0].body.posr().R(), axisR1 );
dCalcVectorCross3( q, ax1, axP );
- info._J[info.J1ap+0] = axP.get0();
- info._J[info.J1ap+1] = axP.get1();
- info._J[info.J1ap+2] = axP.get2();
- info._J[info.J1ap+s+0] = q.get0();
- info._J[info.J1ap+s+1] = q.get1();
- info._J[info.J1ap+s+2] = q.get2();
+ info.setJ1a(0, axP);
+ info.setJ1a(1, q);
if ( node[1].body!=null )
{
- info._J[info.J2ap+0] = -axP.get0();
- info._J[info.J2ap+1] = -axP.get1();
- info._J[info.J2ap+2] = -axP.get2();
- info._J[info.J2ap+s+0] = -q.get0();
- info._J[info.J2ap+s+1] = -q.get1();
- info._J[info.J2ap+s+2] = -q.get2();
+ info.setJ2aNegated(0, axP);
+ info.setJ2aNegated(1, q);
}
@@ -453,34 +441,29 @@ void getSureMaxInfo( SureMaxInfo info )
// Coeff for 2er line of: J1a => dist x q, J2a => - anchor2 x q
- dCalcVectorCross3(info._J, ( info.J1ap ) + s2, dist, ax1 );
+ DVector3 v = new DVector3();
+ dCalcVectorCross3(v, dist, ax1 );
+ info.setJ1a(2, v);
- dCalcVectorCross3(info._J, ( info.J1ap ) + s3, dist, q );
+ dCalcVectorCross3(v, dist, q );
+ info.setJ1a(3, v);
-
- info._J[info.J1lp+s2+0] = ax1.get0();
- info._J[info.J1lp+s2+1] = ax1.get1();
- info._J[info.J1lp+s2+2] = ax1.get2();
-
- info._J[info.J1lp+s3+0] = q.get0();
- info._J[info.J1lp+s3+1] = q.get1();
- info._J[info.J1lp+s3+2] = q.get2();
+ info.setJ1l(2, ax1);
+ info.setJ1l(3, q);
if ( node[1].body!=null )
{
// ax2 x anchor2 instead of anchor2 x ax2 since we want the negative value
- dCalcVectorCross3(info._J, ( info.J2ap ) + s2, ax2, wanchor2 ); // since ax1 == ax2
+ dCalcVectorCross3(v, ax2, wanchor2 ); // since ax1 == ax2
+ info.setJ2a(2, v);
// The cross product is in reverse order since we want the negative value
- dCalcVectorCross3(info._J, ( info.J2ap ) + s3, q, wanchor2 );
+ dCalcVectorCross3(v, q, wanchor2 );
+ info.setJ2a(3, v);
- info._J[info.J2lp+s2+0] = -ax1.get0();
- info._J[info.J2lp+s2+1] = -ax1.get1();
- info._J[info.J2lp+s2+2] = -ax1.get2();
+ info.setJ2lNegated(2, ax1);
+ info.setJ2lNegated(3, q);
- info._J[info.J2lp+s3+0] = -q.get0();
- info._J[info.J2lp+s3+1] = -q.get1();
- info._J[info.J2lp+s3+2] = -q.get2();
}
diff --git a/core/src/main/java/org/ode4j/ode/internal/joints/DxJointPU.java b/core/src/main/java/org/ode4j/ode/internal/joints/DxJointPU.java
index 27146aab..2cf5fe7e 100644
--- a/core/src/main/java/org/ode4j/ode/internal/joints/DxJointPU.java
+++ b/core/src/main/java/org/ode4j/ode/internal/joints/DxJointPU.java
@@ -27,7 +27,6 @@
import static org.ode4j.ode.OdeConstants.dInfinity;
import static org.ode4j.ode.OdeMath.dCalcVectorCross3;
import static org.ode4j.ode.OdeMath.dCalcVectorDot3;
-import static org.ode4j.ode.OdeMath.dCopyNegatedVector3;
import static org.ode4j.ode.OdeMath.dMultiply0_331;
import static org.ode4j.ode.OdeMath.dNormalize3;
import static org.ode4j.ode.OdeMath.dPlaneSpace;
@@ -335,8 +334,6 @@ void getSureMaxInfo( SureMaxInfo info )
public void
getInfo2( double worldFPS, double worldERP, DxJoint.Info2Descr info )
{
- final int s1 = info.rowskip();
- final int s2 = 2 * s1;
final double k = worldFPS * worldERP;
// ======================================================================
@@ -350,12 +347,11 @@ void getSureMaxInfo( SureMaxInfo info )
dCalcVectorCross3(uniPerp,ax1,ax2);
dNormalize3( uniPerp );
//dCopyVector3( info.J1a , uniPerp );
- uniPerp.wrapSet(info._J, info.J1ap);
+ info.setJ1a(0, uniPerp);
if ( node[1].body != null )
{
//dCopyNegatedVector3( info.J2a , uniPerp );
- uniPerp.scale(-1).wrapSet(info._J, info.J2ap);
- //TODO this changes uniPerp!!!! (TZ)
+ info.setJ2aNegated(0, uniPerp);
}
// Corrective velocity attempting to keep uni axes perpendicular
double val = dCalcVectorDot3( ax1, ax2 );
@@ -382,23 +378,28 @@ void getSureMaxInfo( SureMaxInfo info )
dPlaneSpace(axP,p,q);
//dCopyVector3(( info.J1l ) + s1, p );
- p.wrapSet(info._J, info.J1lp + s1);
- //dCopyVector3(( info.J1l ) + s2, q );
- p.wrapSet(info._J, info.J1lp + s2);
+ info.setJ1l(1, p);
+ //dCopyVector3(( info.J1l ) + s2, q );
+ info.setJ1l(2, q);
// Make the anchors be body local
// Aliasing isn't a problem here.
//dSubtractVectors3(an1,an1,node[0].body.posr().pos());
an1.sub(node[0].body.posr().pos());
- dCalcVectorCross3(info._J, info.J1ap + s1, an1, p );
- dCalcVectorCross3(info._J, info.J1ap + s2, an1, q );
+ DVector3 v = new DVector3();
+ dCalcVectorCross3(v, an1, p );
+ info.setJ1a(1, v);
+ dCalcVectorCross3(v, an1, q );
+ info.setJ1a(2, v);
if (node[1].body != null) {
- dCopyNegatedVector3(info._J, info.J2lp + s1, p );
- dCopyNegatedVector3(info._J, info.J2lp + s2, q );
+ info.setJ2lNegated(1, p);
+ info.setJ2lNegated(2, q);
//dSubtractVectors3(an2,an2,node[1].body.posr().pos());
an2.sub(node[1].body.posr().pos());
- dCalcVectorCross3(info._J, info.J2ap + s1, p, an2 );
- dCalcVectorCross3(info._J, info.J2ap + s2, q, an2 );
+ dCalcVectorCross3(v, p, an2 );
+ info.setJ2a(1, v);
+ dCalcVectorCross3(v, q, an2 );
+ info.setJ2a(2, v);
}
info.setC(1, k * dCalcVectorDot3( p, sep ) );
diff --git a/core/src/main/java/org/ode4j/ode/internal/joints/DxJointPiston.java b/core/src/main/java/org/ode4j/ode/internal/joints/DxJointPiston.java
index 85f5f8f9..0aa5c42f 100644
--- a/core/src/main/java/org/ode4j/ode/internal/joints/DxJointPiston.java
+++ b/core/src/main/java/org/ode4j/ode/internal/joints/DxJointPiston.java
@@ -27,8 +27,6 @@
import static org.ode4j.ode.OdeConstants.dInfinity;
import static org.ode4j.ode.OdeMath.dCalcVectorCross3;
import static org.ode4j.ode.OdeMath.dCalcVectorDot3;
-import static org.ode4j.ode.OdeMath.dCopyNegatedVector3;
-import static org.ode4j.ode.OdeMath.dCopyVector3;
import static org.ode4j.ode.OdeMath.dMultiply0_331;
import static org.ode4j.ode.OdeMath.dMultiply1_331;
import static org.ode4j.ode.OdeMath.dPlaneSpace;
@@ -296,9 +294,6 @@ void getSureMaxInfo( SureMaxInfo info )
public void
getInfo2 ( double worldFPS, double worldERP, DxJoint.Info2Descr info )
{
- final int s0 = 0;
- final int s1 = info.rowskip();
- final int s2 = 2 * s1, s3 = 3 * s1 /*, s4=4*s1*/;
final double k = worldFPS * worldERP;
@@ -388,16 +383,16 @@ void getSureMaxInfo( SureMaxInfo info )
dPlaneSpace ( ax1, p, q );
// LHS
- dCopyVector3 ( info._J, ( info.J1ap ) + s0, p );
- dCopyVector3 ( info._J, ( info.J1ap ) + s1, q );
+ info.setJ1a(0, p);
+ info.setJ1a(1, q);
DVector3 b = new DVector3();
if ( node[1].body !=null)
{
// LHS
// info.J2a[s0+i] = -p[i]
- dCopyNegatedVector3 ( info._J, ( info.J2ap ) + s0, p );
- dCopyNegatedVector3 ( info._J, ( info.J2ap ) + s1, q );
+ info.setJ2aNegated(0, p);
+ info.setJ2aNegated(1, q);
// Some math for the RHS
@@ -441,25 +436,29 @@ void getSureMaxInfo( SureMaxInfo info )
// Coeff for 2er line of: J1l => q, J2l => -q
// Coeff for 1er line of: J1a => dist x p, J2a => p x anchor2
// Coeff for 2er line of: J1a => dist x q, J2a => q x anchor2
+ DVector3 v = new DVector3();
+ dCalcVectorCross3 ( v, dist, p );
+ info.setJ1a(2, v);
+
+ dCalcVectorCross3 ( v, dist, q );
+ info.setJ1a(3, v);
- dCalcVectorCross3 ( info._J, info.J1ap + s2, dist, p );
-
- dCalcVectorCross3 ( info._J, ( info.J1ap ) + s3, dist, q );
-
- dCopyVector3 ( info._J, ( info.J1lp ) + s2 , p );
- dCopyVector3 ( info._J, ( info.J1lp ) + s3, q );
+ info.setJ1l(2, p);
+ info.setJ1l(3, q);
if ( node[1].body!=null )
{
// q x anchor2 instead of anchor2 x q since we want the negative value
- dCalcVectorCross3 ( info._J, ( info.J2ap ) + s2, p, lanchor2 );
+ dCalcVectorCross3 ( v, p, lanchor2 );
+ info.setJ2a(2, v);
// The cross product is in reverse order since we want the negative value
- dCalcVectorCross3 ( info._J, ( info.J2ap ) + s3, q, lanchor2 );
+ dCalcVectorCross3 ( v, q, lanchor2 );
+ info.setJ2a(3, v);
// info.J2l[s2+i] = -p[i];
- dCopyNegatedVector3 ( info._J, ( info.J2lp ) + s2, p );
- dCopyNegatedVector3 ( info._J, ( info.J2lp ) + s3, q );
+ info.setJ2lNegated(2, p);
+ info.setJ2lNegated(3, q);
}
diff --git a/core/src/main/java/org/ode4j/ode/internal/joints/DxJointPlane2D.java b/core/src/main/java/org/ode4j/ode/internal/joints/DxJointPlane2D.java
index cb1e417d..e901f7ab 100644
--- a/core/src/main/java/org/ode4j/ode/internal/joints/DxJointPlane2D.java
+++ b/core/src/main/java/org/ode4j/ode/internal/joints/DxJointPlane2D.java
@@ -118,9 +118,6 @@ void getSureMaxInfo( SureMaxInfo info )
public void
getInfo2( double worldFPS, double worldERP, DxJoint.Info2Descr info )
{
- int r0 = 0;
- int r1 = info.rowskip();
- int r2 = 2 * r1;
double eps = worldFPS * worldERP;
/*
@@ -147,20 +144,25 @@ void getSureMaxInfo( SureMaxInfo info )
// VoXYZ( info.J1a.v[r0], OP.EQ , 0, 0, 0 );
// VoXYZ( info.J1a.v[r1], OP.EQ , 1, 0, 0 );
// VoXYZ( info.J1a.v[r2], OP.EQ , 0, 1, 0 );
- int rx = info.J1lp + r0;
- info._J[rx] = 0; info._J[rx+1] = 0; info._J[rx+2] = 1;
- rx = info.J1lp + r1;
- info._J[rx] = 0; info._J[rx+1] = 0; info._J[rx+2] = 0;
- rx = info.J1lp + r2;
- info._J[rx] = 0; info._J[rx+1] = 0; info._J[rx+2] = 0;
-// info.J1l.set(0, 0, 1, 0,0,0, 0,0,0);
-// info.J1a.set(0, 0, 0, 1,0,0, 0,1,0 );
- rx = info.J1ap + r0;
- info._J[rx] = 0; info._J[rx+1] = 0; info._J[rx+2] = 0;
- rx = info.J1ap + r1;
- info._J[rx] = 1; info._J[rx+1] = 0; info._J[rx+2] = 0;
- rx = info.J1ap + r2;
- info._J[rx] = 0; info._J[rx+1] = 1; info._J[rx+2] = 0;
+ info.setJ1l(0, 0, 0);
+ info.setJ1l(0, 1, 0);
+ info.setJ1l(0, 2, 1);
+ info.setJ1l(1, 0, 0);
+ info.setJ1l(1, 1, 0);
+ info.setJ1l(1, 2, 0);
+ info.setJ1l(2, 0, 0);
+ info.setJ1l(2, 1, 0);
+ info.setJ1l(2, 2, 0);
+
+ info.setJ1a(0, 0, 0);
+ info.setJ1a(0, 1, 0);
+ info.setJ1a(0, 2, 0);
+ info.setJ1a(1, 0, 1);
+ info.setJ1a(1, 1, 0);
+ info.setJ1a(1, 2, 0);
+ info.setJ1a(2, 0, 0);
+ info.setJ1a(2, 1, 1);
+ info.setJ1a(2, 2, 0);
// error correction (against drift):
diff --git a/core/src/main/java/org/ode4j/ode/internal/joints/DxJointSlider.java b/core/src/main/java/org/ode4j/ode/internal/joints/DxJointSlider.java
index fc0a8ab5..4a21ee83 100644
--- a/core/src/main/java/org/ode4j/ode/internal/joints/DxJointSlider.java
+++ b/core/src/main/java/org/ode4j/ode/internal/joints/DxJointSlider.java
@@ -174,8 +174,6 @@ else if ( pos >= limot.histop )
public void
getInfo2 ( double worldFPS, double worldERP, DxJoint.Info2Descr info )
{
- int i, s = info.rowskip();
- int s3 = 3 * s, s4 = 4 * s;
// pull out pos and R for both bodies. also get the `connection'
// vector pos2-pos1.
@@ -218,19 +216,19 @@ else if ( pos >= limot.histop )
dCalcVectorCross3 ( tmp, c, p );
tmp.scale(0.5);
- for ( i = 0; i < 3; i++ ) info._J[info.J1ap+s3+i] = tmp.get(i);
- for ( i = 0; i < 3; i++ ) info._J[info.J2ap+s3+i] = tmp.get(i);
+ info.setJ1a(3, tmp);
+ info.setJ2a(3, tmp);
//dCalcVectorCross3 ( tmp, = 0.5 * , c, q );
dCalcVectorCross3 ( tmp, c, p );
tmp.scale(0.5);
- for ( i = 0; i < 3; i++ ) info._J[info.J1ap+s4+i] = tmp.get(i);
- for ( i = 0; i < 3; i++ ) info._J[info.J2ap+s4+i] = tmp.get(i);
- for ( i = 0; i < 3; i++ ) info._J[info.J2lp+s3+i] = -p.get(i);
- for ( i = 0; i < 3; i++ ) info._J[info.J2lp+s4+i] = -q.get(i);
+ info.setJ1a(4, tmp);
+ info.setJ2a(4, tmp);
+ info.setJ2lNegated(3, p);
+ info.setJ2lNegated(4, q);
}
- for ( i = 0; i < 3; i++ ) info._J[info.J1lp+s3+i] = p.get(i);
- for ( i = 0; i < 3; i++ ) info._J[info.J1lp+s4+i] = q.get(i);
+ info.setJ1l(3, p);
+ info.setJ1l(4, q);
// compute last two elements of right hand side. we want to align the offset
// point (in body 2's frame) with the center of body 1.
diff --git a/core/src/main/java/org/ode4j/ode/internal/joints/DxJointTransmission.java b/core/src/main/java/org/ode4j/ode/internal/joints/DxJointTransmission.java
index e4890845..0e4a1ea5 100644
--- a/core/src/main/java/org/ode4j/ode/internal/joints/DxJointTransmission.java
+++ b/core/src/main/java/org/ode4j/ode/internal/joints/DxJointTransmission.java
@@ -410,12 +410,14 @@ private static final double clamp(double x, double minX, double maxX) {
r[i].eqDiff(c[i], p[i]);//dSubtractVectors3 (r[i], c[i], p[i]);
}
- dCalcVectorCross3(info._J, info.J1ap, r[0], l[0]);
- dCalcVectorCross3(info._J, info.J2ap, l[1], r[1]);
+ dCalcVectorCross3(v, r[0], l[0]);
+ info.setJ1a(0, v);
+ dCalcVectorCross3(v, l[1], r[1]);
+ info.setJ2a(0, v);
- l[0].wrapSet(info._J, info.J1lp);//dCopyVector3(info.J1l, l[0]);
+ info.setJ1l(0, l[0]);
//dCopyNegatedVector3(info.J2l, l[1]);
- l[1].scale(-1).wrapSet(info._J, info.J2lp);
+ info.setJ2lNegated(0, l[1]);
if (delta > 0) {
if (backlash > 0) {
diff --git a/core/src/main/java/org/ode4j/ode/internal/joints/DxJointUniversal.java b/core/src/main/java/org/ode4j/ode/internal/joints/DxJointUniversal.java
index 65ab7cc0..bbbfc3be 100644
--- a/core/src/main/java/org/ode4j/ode/internal/joints/DxJointUniversal.java
+++ b/core/src/main/java/org/ode4j/ode/internal/joints/DxJointUniversal.java
@@ -365,17 +365,11 @@ void getSureMaxInfo( SureMaxInfo info )
dCalcVectorCross3( p, ax1, ax2_temp );
dNormalize3( p );
- int s3 = 3 * info.rowskip();
-
- info._J[info.J1ap+s3+0] = p.get0();
- info._J[info.J1ap+s3+1] = p.get1();
- info._J[info.J1ap+s3+2] = p.get2();
+ info.setJ1a(3, p);
if ( node[1].body != null)
{
- info._J[info.J2ap+s3+0] = -p.get0();
- info._J[info.J2ap+s3+1] = -p.get1();
- info._J[info.J2ap+s3+2] = -p.get2();
+ info.setJ2aNegated(3, p);
}
// compute the right hand side of the constraint equation. set relative