Exemplo n.º 1
0
 //	void setAnchors( dxJoint j, double x, double y, double z,
 //	dVector3 anchor1, dVector3 anchor2 )
 final void setAnchors(DVector3C xyz, DVector3 anchor1, DVector3 anchor2) {
   if (node[0].body != null) {
     /// double[] q = new double[4];
     DVector3 q = new DVector3();
     //			q.v[0] = x - node[0].body._posr.pos.v[0];
     //			q.v[1] = y - node[0].body._posr.pos.v[1];
     //			q.v[2] = z - node[0].body._posr.pos.v[2];
     //			q.v[3] = 0;
     q.eqDiff(xyz, node[0].body.posr().pos());
     dMultiply1_331(anchor1, node[0].body.posr().R(), q);
     if (node[1].body != null) {
       //				q.v[0] = x - node[1].body._posr.pos.v[0];
       //				q.v[1] = y - node[1].body._posr.pos.v[1];
       //				q.v[2] = z - node[1].body._posr.pos.v[2];
       //				q.v[3] = 0;
       q.eqDiff(xyz, node[1].body.posr().pos());
       dMultiply1_331(anchor2, node[1].body.posr().R(), q);
     } else {
       //				anchor2.v[0] = x;
       //				anchor2.v[1] = y;
       //				anchor2.v[2] = z;
       anchor2.set(xyz);
     }
   }
   //		anchor1.v[3] = 0;
   //		anchor2.v[3] = 0;
 }
Exemplo n.º 2
0
  void setBall2(
      DxJoint joint, Info2 info, DVector3 anchor1, DVector3 anchor2, DVector3 axis, double erp1) {
    // 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();
    dPlaneSpace(axis, q1, q2);

    // set jacobian
    //		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);
    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);
    if (joint.node[1].body != null) {
      //			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);
      //	        dMultiply0_331( a2, joint->node[1].body->posr.R, anchor2 );
      //	        dReal *J2a = info->J2a;
      //	        dCalcVectorCross3( J2a, a2, axis );
      //	        dNegateVector3( J2a );
      //	        dReal *J2a_plus_s = J2a + s;
      //	        dCalcVectorCross3( J2a_plus_s, a2, q1 );
      //	        dNegateVector3( J2a_plus_s );
      //	        dReal *J2a_plus_2s = J2a_plus_s + s;
      //	        dCalcVectorCross3( J2a_plus_2s, a2, q2 );
      //	        dNegateVector3( J2a_plus_2s );
      dMultiply0_331(a2, joint.node[1].body._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);
      // dReal *J2a_plus_s = J2a + s;
      dCalcVectorCross3(J, J2ap + s, a2, q1);
      dNegateVector3(J, J2ap + s);
      // dReal *J2a_plus_2s = J2a_plus_s + s;
      dCalcVectorCross3(J, J2ap + 2 * s, a2, q2);
      dNegateVector3(J, J2ap + 2 * s);
    }

    // set right hand side - measure error along (axis,q1,q2)
    double k1 = info.fps * erp1;
    double k = info.fps * info.erp;

    // for ( i = 0; i < 3; i++ ) a1.v[i] += joint.node[0].body._posr.pos.v[i];
    a1.add(joint.node[0].body.posr().pos());
    if (joint.node[1].body != null) {
      //			for ( i = 0; i < 3; i++ ) a2.v[i] += joint.node[1].body._posr.pos.v[i];
      a2.add(joint.node[1].body.posr().pos());
      DVector3 a2_minus_a1 = new DVector3();
      a2_minus_a1.eqDiff(a2, a1);
      info.setC(0, k1 * (dCalcVectorDot3(axis, a2_minus_a1)));
      info.setC(1, k * (dCalcVectorDot3(q1, a2_minus_a1)));
      info.setC(2, k * (dCalcVectorDot3(q2, a2_minus_a1)));
    } else {
      DVector3 anchor2_minus_a1 = new DVector3();
      anchor2_minus_a1.eqDiff(anchor2, a1);
      info.setC(0, k1 * (dCalcVectorDot3(axis, anchor2_minus_a1)));
      info.setC(1, k * (dCalcVectorDot3(q1, anchor2_minus_a1)));
      info.setC(2, k * (dCalcVectorDot3(q2, anchor2_minus_a1)));
    }
  }