Exemplo n.º 1
0
  public void dJointAttach(DBody b1, DBody b2) {
    DxBody body1 = (DxBody) b1;
    DxBody body2 = (DxBody) b2;
    // check arguments
    // dUASSERT (joint,"bad joint argument");
    dUASSERT(body1 == null || body1 != body2, "can't have body1==body2");
    DxWorld world = this.world;
    dUASSERT(
        (body1 == null || body1.getWorld() == world)
            && (body2 == null || body2.getWorld() == world),
        "joint and bodies must be in same world");

    // check if the joint can not be attached to just one body
    dUASSERT(
        !((flags & dJOINT_TWOBODIES) != 0 && ((body1 != null) ^ (body2 != null))),
        "joint can not be attached to just one body");

    // remove any existing body attachments
    if (node[0].body != null || node[1].body != null) {
      removeJointReferencesFromAttachedBodies();
    }

    // if a body is zero, make sure that it is body2, so 0 --> node[1].body
    if (body1 == null) {
      body1 = body2;
      body2 = null;
      flags |= dJOINT_REVERSE;
    } else {
      flags &= (~dJOINT_REVERSE);
    }

    // attach to new bodies
    node[0].body = body1;
    node[1].body = body2;
    if (body1 != null) {
      node[1].next = body1.firstjoint.get();
      body1.firstjoint.set(node[1]);
    } else node[1].next = null;
    if (body2 != null) {
      node[0].next = body2.firstjoint.get();
      body2.firstjoint.set(node[0]);
    } else {
      node[0].next = null;
    }

    // Since the bodies are now set.
    // Calculate the values depending on the bodies.
    // Only need to calculate relative value if a body exist
    if (body1 != null || body2 != null) setRelativeValues();
  }
Exemplo n.º 2
0
  void setBall(DxJoint joint, Info2 info, DVector3 anchor1, DVector3 anchor2) {
    // 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;
    dMultiply0_331(a1, joint.node[0].body.posr().R(), anchor1);
    //    dCROSSMAT( info.J1a, a1, s, -, + );
    dSetCrossMatrixMinus(info._J, info.J1ap, a1, s);
    if (joint.node[1].body != null) {
      info._J[info.J2lp + 0] = -1;
      info._J[info.J2lp + s + 1] = -1;
      info._J[info.J2lp + 2 * s + 2] = -1;
      dMultiply0_331(a2, joint.node[1].body.posr().R(), anchor2);
      dSetCrossMatrixPlus(info._J, info.J2ap, a2, s);
    }

    DxBody b0 = joint.node[0].body;
    DxBody b1 = joint.node[1].body;

    // set right hand side
    double k = info.fps * info.erp;
    if (joint.node[1].body != null) {
      //			for ( int j = 0; j < 3; j++ )
      //			{
      //				info.setC(j, k * ( a2.v[j] + b1.posr().pos().v[j] -
      //						a1.v[j] - b0._posr.pos.v[j] ));
      //			}
      info.setC(0, k * (a2.get0() + b1.posr().pos().get0() - a1.get0() - b0.posr().pos().get0()));
      info.setC(1, k * (a2.get1() + b1.posr().pos().get1() - a1.get1() - b0.posr().pos().get1()));
      info.setC(2, k * (a2.get2() + b1.posr().pos().get2() - a1.get2() - b0.posr().pos().get2()));
    } else {
      //			for ( int j = 0; j < 3; j++ )
      //			{
      //				info.setC(j, k * ( anchor2.v[j] - a1.v[j] -
      //						b0._posr.pos.v[j] ));
      //			}
      info.setC(0, k * (anchor2.get0() - a1.get0() - b0.posr().pos().get0()));
      info.setC(1, k * (anchor2.get1() - a1.get1() - b0.posr().pos().get1()));
      info.setC(2, k * (anchor2.get2() - a1.get2() - b0.posr().pos().get2()));
    }
  }