Examples of com.bulletphysics.dynamics.constraintsolver.Generic6DofConstraint

Package com.bulletphysics.dynamics.constraintsolver

Examples of com.bulletphysics.dynamics.constraintsolver.Generic6DofConstraint

com.bulletphysics.dynamics.constraintsolver.Generic6DofConstraint
Generic6DofConstraint between two rigidbodies each with a pivot point that descibes the axis location in local space.
Generic6DofConstraint can leave any of the 6 degree of freedom "free" or "locked". Currently this limit supports rotational motors.
- For linear limits, use {@link #setLinearUpperLimit}, {@link #setLinearLowerLimit}. You can set the parameters with the {@link TranslationalLimitMotor} structure accsesiblethrough the {@link #getTranslationalLimitMotor} method.At this moment translational motors are not supported. May be in the future.
- For angular limits, use the {@link RotationalLimitMotor} structure for configuringthe limit. This is accessible through {@link #getRotationalLimitMotor} method,this brings support for limit parameters and motors.
- Angulars limits have these possible ranges:
  
  AXIS MIN ANGLE MAX ANGLE
  X -PI PI
  Y -PI/2 PI/2
  Z -PI/2 PI/2
@author jezek2


    Vector3f tmp = new Vector3f();


    ///////////////////////////// SETTING THE CONSTRAINTS /////////////////////////////////////////////7777
    // Now setup the constraints
    Generic6DofConstraint joint6DOF;
    Transform localA = new Transform(), localB = new Transform();
    boolean useLinearReferenceFrameA = true;
    /// ******* SPINE HEAD ******** ///
    {
      localA.setIdentity();
      localB.setIdentity();


      localA.origin.set(0f, -0.30f * this.buildScale, 0f);


      localB.origin.set(0f, 0.14f * this.buildScale, 0f);


      joint6DOF = new Generic6DofConstraint(
          bodies[BodyPart.BODYPART_SPINE.ordinal()],
          bodies[BodyPart.BODYPART_HEAD.ordinal()], localA, localB,
          useLinearReferenceFrameA);


      //#ifdef RIGID
      //joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
      //joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
      //#else


      tmp.set(-BulletGlobals.SIMD_PI * 0.2f, -BulletGlobals.FLT_EPSILON,
          -BulletGlobals.SIMD_PI * 0.1f);
      joint6DOF.setAngularLowerLimit(tmp);
      tmp.set(BulletGlobals.SIMD_PI * 0.1f, BulletGlobals.FLT_EPSILON,
          BulletGlobals.SIMD_PI * 0.10f);
      joint6DOF.setAngularUpperLimit(tmp);
      //#endif
      joints[JointType.JOINT_SPINE_HEAD.ordinal()] = joint6DOF;
      ownerWorld.addConstraint(
          joints[JointType.JOINT_SPINE_HEAD.ordinal()], true);
    }
    /// *************************** ///


    /// ******* SPINE 2D ******** ///
    {
      localA.setIdentity();
      localB.setIdentity();


      localA.origin.set(0f, 0f, 0f);


      localB.origin.set(0f, 0f, 0f);


      CollisionShape ZeroShape = null;
      GLOBAL.jBullet.ZeroBody = new RigidBody(0.0f, null, ZeroShape);


      Transform Rotation = new Transform();
      Rotation.setIdentity();
      GLOBAL.jBullet.ZeroBody.setWorldTransform(Rotation);


      joint6DOF = new Generic6DofConstraint(
          bodies[BodyPart.BODYPART_SPINE.ordinal()],
          GLOBAL.jBullet.ZeroBody, localA, localB, false);


      joint6DOF.setLimit(0, 1, 0); // Disable X axis limits
      joint6DOF.setLimit(1, 1, 0); // Disable Y axis limits
      joint6DOF.setLimit(2, 0, 0); // Set the Z axis to always be equal to zero
      joint6DOF.setLimit(3, 0, 0); // Disable X rotational axes
      joint6DOF.setLimit(4, 0, 0); // Disable Y rotational axes
      joint6DOF.setLimit(5, 1, 0); // Uncap the rotational axes
      //#endif
      joints[JointType.JOINT_PLANE.ordinal()] = joint6DOF;
      ownerWorld.addConstraint(joints[JointType.JOINT_PLANE.ordinal()],
          true);
    }
    /// *************************** ///


    if (hasArms) {
      /// ******* LEFT SHOULDER ******** ///
      {
        localA.setIdentity();
        localB.setIdentity();


        localA.origin.set(0f, 0.15f * -this.buildScale,
            0.2f * this.buildScale);


        MatrixUtil.setEulerZYX(localB.basis,
            BulletGlobals.SIMD_HALF_PI, 0,
            -BulletGlobals.SIMD_HALF_PI);
        localB.origin.set(0f, 0.18f * this.buildScale, 0f);


        joint6DOF = new Generic6DofConstraint(
            bodies[BodyPart.BODYPART_SPINE.ordinal()],
            bodies[BodyPart.BODYPART_LEFT_UPPER_ARM.ordinal()],
            localA, localB, useLinearReferenceFrameA);


        //#ifdef RIGID
        //joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
        //joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
        //#else
        tmp.set(-BulletGlobals.SIMD_PI * 0.8f,
            -BulletGlobals.FLT_EPSILON,
            -BulletGlobals.SIMD_PI * 0.5f);
        joint6DOF.setAngularLowerLimit(tmp);
        tmp.set(BulletGlobals.SIMD_PI * 0.8f,
            BulletGlobals.FLT_EPSILON, BulletGlobals.SIMD_PI * 0.5f);
        joint6DOF.setAngularUpperLimit(tmp);
        //#endif
        joints[JointType.JOINT_LEFT_SHOULDER.ordinal()] = joint6DOF;
        ownerWorld.addConstraint(
            joints[JointType.JOINT_LEFT_SHOULDER.ordinal()], true);
      }
      /// *************************** ///


      /// ******* RIGHT SHOULDER ******** ///
      {
        localA.setIdentity();
        localB.setIdentity();


        localA.origin.set(0f, 0.15f * -this.buildScale, -0.2f
            * this.buildScale);
        MatrixUtil.setEulerZYX(localB.basis, 0, 0,
            BulletGlobals.SIMD_HALF_PI);
        localB.origin.set(0f, 0.18f * this.buildScale, 0f);
        joint6DOF = new Generic6DofConstraint(
            bodies[BodyPart.BODYPART_SPINE.ordinal()],
            bodies[BodyPart.BODYPART_RIGHT_UPPER_ARM.ordinal()],
            localA, localB, useLinearReferenceFrameA);


        //#ifdef RIGID
        //joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
        //joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
        //#else
        tmp.set(-BulletGlobals.SIMD_PI * 0.8f,
            -BulletGlobals.SIMD_EPSILON,
            -BulletGlobals.SIMD_PI * 0.5f);
        joint6DOF.setAngularLowerLimit(tmp);
        tmp.set(BulletGlobals.SIMD_PI * 0.8f,
            BulletGlobals.SIMD_EPSILON,
            BulletGlobals.SIMD_PI * 0.5f);
        joint6DOF.setAngularUpperLimit(tmp);
        //#endif
        joints[JointType.JOINT_RIGHT_SHOULDER.ordinal()] = joint6DOF;
        ownerWorld.addConstraint(
            joints[JointType.JOINT_RIGHT_SHOULDER.ordinal()], true);
      }
      /// *************************** ///


      /// ******* LEFT ELBOW ******** ///
      {
        localA.setIdentity();
        localB.setIdentity();


        localA.origin.set(0f, -0.18f * this.buildScale, 0f);
        localB.origin.set(0f, -0.14f * this.buildScale, 0f);
        joint6DOF = new Generic6DofConstraint(
            bodies[BodyPart.BODYPART_LEFT_UPPER_ARM.ordinal()],
            bodies[BodyPart.BODYPART_LEFT_LOWER_ARM.ordinal()],
            localA, localB, useLinearReferenceFrameA);


        //#ifdef RIGID
        //joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
        //joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
        //#else
        tmp.set(-BulletGlobals.SIMD_EPSILON,
            -BulletGlobals.SIMD_EPSILON,
            -BulletGlobals.SIMD_EPSILON);
        joint6DOF.setAngularLowerLimit(tmp);
        tmp.set(BulletGlobals.SIMD_PI * 0.7f,
            BulletGlobals.SIMD_EPSILON, BulletGlobals.SIMD_EPSILON);
        joint6DOF.setAngularUpperLimit(tmp);
        //#endif
        joints[JointType.JOINT_LEFT_ELBOW.ordinal()] = joint6DOF;
        ownerWorld.addConstraint(
            joints[JointType.JOINT_LEFT_ELBOW.ordinal()], true);
      }
      /// *************************** ///


      /// ******* RIGHT ELBOW ******** ///
      {
        localA.setIdentity();
        localB.setIdentity();


        localA.origin.set(0f, -0.18f * this.buildScale, 0f);
        localB.origin.set(0f, -0.14f * this.buildScale, 0f);
        joint6DOF = new Generic6DofConstraint(
            bodies[BodyPart.BODYPART_RIGHT_UPPER_ARM.ordinal()],
            bodies[BodyPart.BODYPART_RIGHT_LOWER_ARM.ordinal()],
            localA, localB, useLinearReferenceFrameA);


        //#ifdef RIGID
        //joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
        //joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
        //#else
        tmp.set(-BulletGlobals.SIMD_EPSILON,
            -BulletGlobals.SIMD_EPSILON,
            -BulletGlobals.SIMD_EPSILON);
        joint6DOF.setAngularLowerLimit(tmp);
        tmp.set(BulletGlobals.SIMD_PI * 0.7f,
            BulletGlobals.SIMD_EPSILON, BulletGlobals.SIMD_EPSILON);
        joint6DOF.setAngularUpperLimit(tmp);
        //#endif


        joints[JointType.JOINT_RIGHT_ELBOW.ordinal()] = joint6DOF;
        ownerWorld.addConstraint(
            joints[JointType.JOINT_RIGHT_ELBOW.ordinal()], true);
      }
      /// *************************** ///
    }


    /// ******* PELVIS ******** ///
    {
      localA.setIdentity();
      localB.setIdentity();


      MatrixUtil.setEulerZYX(localA.basis, 0, BulletGlobals.SIMD_HALF_PI,
          0);
      localA.origin.set(0f, -0.15f * this.buildScale, 0f);
      MatrixUtil.setEulerZYX(localB.basis, 0, BulletGlobals.SIMD_HALF_PI,
          0);
      localB.origin.set(0f, 0.15f * this.buildScale, 0f);
      joint6DOF = new Generic6DofConstraint(
          bodies[BodyPart.BODYPART_PELVIS.ordinal()],
          bodies[BodyPart.BODYPART_SPINE.ordinal()], localA, localB,
          useLinearReferenceFrameA);


      //#ifdef RIGID
      //joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
      //joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
      //#else
      tmp.set(-BulletGlobals.SIMD_PI * 0.1f, -BulletGlobals.SIMD_EPSILON,
          -BulletGlobals.SIMD_PI * 0.1f);
      joint6DOF.setAngularLowerLimit(tmp);
      tmp.set(BulletGlobals.SIMD_PI * 0.2f, BulletGlobals.SIMD_EPSILON,
          BulletGlobals.SIMD_PI * 0.15f);
      joint6DOF.setAngularUpperLimit(tmp);
      //#endif
      joints[JointType.JOINT_PELVIS_SPINE.ordinal()] = joint6DOF;
      ownerWorld.addConstraint(
          joints[JointType.JOINT_PELVIS_SPINE.ordinal()], true);
    }
    /// *************************** ///


    /// ******* LEFT HIP ******** ///
    {
      localA.setIdentity();
      localB.setIdentity();


      localA.origin.set(0f, 0.10f * this.buildScale,
          0.08f * this.buildScale);


      localB.origin.set(0f, -0.225f * this.buildScale, 0f);


      joint6DOF = new Generic6DofConstraint(
          bodies[BodyPart.BODYPART_PELVIS.ordinal()],
          bodies[BodyPart.BODYPART_LEFT_UPPER_LEG.ordinal()], localA,
          localB, useLinearReferenceFrameA);


      //#ifdef RIGID
      //joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
      //joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
      //#else
      tmp.set(-BulletGlobals.SIMD_EPSILON, -BulletGlobals.SIMD_EPSILON,
          -BulletGlobals.SIMD_HALF_PI * 0.5f);
      joint6DOF.setAngularLowerLimit(tmp);
      tmp.set(BulletGlobals.SIMD_HALF_PI * 0.6f,
          BulletGlobals.SIMD_EPSILON,
          BulletGlobals.SIMD_HALF_PI * 1.3f); //bend foward
      joint6DOF.setAngularUpperLimit(tmp);
      //#endif
      joints[JointType.JOINT_LEFT_HIP.ordinal()] = joint6DOF;
      ownerWorld.addConstraint(
          joints[JointType.JOINT_LEFT_HIP.ordinal()], true);
    }
    /// *************************** ///


    /// ******* RIGHT HIP ******** ///
    {
      localA.setIdentity();
      localB.setIdentity();


      localA.origin.set(0f, 0.10f * this.buildScale, -0.08f
          * this.buildScale);
      localB.origin.set(0f, -0.225f * this.buildScale, 0f);


      joint6DOF = new Generic6DofConstraint(
          bodies[BodyPart.BODYPART_PELVIS.ordinal()],
          bodies[BodyPart.BODYPART_RIGHT_UPPER_LEG.ordinal()],
          localA, localB, useLinearReferenceFrameA);


      //#ifdef RIGID
      //joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
      //joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
      //#else
      tmp.set(-BulletGlobals.SIMD_HALF_PI * 0.6f,
          -BulletGlobals.SIMD_EPSILON,
          -BulletGlobals.SIMD_HALF_PI * 0.5f);
      joint6DOF.setAngularLowerLimit(tmp);
      tmp.set(BulletGlobals.SIMD_EPSILON, BulletGlobals.SIMD_EPSILON,
          BulletGlobals.SIMD_HALF_PI * 1.3f);
      joint6DOF.setAngularUpperLimit(tmp);
      //#endif
      joints[JointType.JOINT_RIGHT_HIP.ordinal()] = joint6DOF;
      ownerWorld.addConstraint(
          joints[JointType.JOINT_RIGHT_HIP.ordinal()], true);
    }
    /// *************************** ///


    /// ******* LEFT KNEE ******** ///
    {
      localA.setIdentity();
      localB.setIdentity();


      localA.origin.set(0f, 0.265f * this.buildScale, 0f);
      localB.origin.set(0f, -0.225f * this.buildScale, 0f);
      joint6DOF = new Generic6DofConstraint(
          bodies[BodyPart.BODYPART_LEFT_UPPER_LEG.ordinal()],
          bodies[BodyPart.BODYPART_LEFT_LOWER_LEG.ordinal()], localA,
          localB, useLinearReferenceFrameA);
      //
      //#ifdef RIGID
      //joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
      //joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
      //#else
      tmp.set(-BulletGlobals.SIMD_EPSILON, -BulletGlobals.SIMD_EPSILON,
          -BulletGlobals.SIMD_PI * .9f);
      joint6DOF.setAngularLowerLimit(tmp);
      tmp.set(BulletGlobals.SIMD_EPSILON, BulletGlobals.SIMD_EPSILON,
          BulletGlobals.SIMD_EPSILON);
      joint6DOF.setAngularUpperLimit(tmp);
      //#endif
      joints[JointType.JOINT_LEFT_KNEE.ordinal()] = joint6DOF;
      ownerWorld.addConstraint(
          joints[JointType.JOINT_LEFT_KNEE.ordinal()], true);
    }
    /// *************************** ///


    /// ******* RIGHT KNEE ******** ///
    {
      localA.setIdentity();
      localB.setIdentity();
      HingeConstraint jointHinge = null;
      localA.origin.set(0f, 0.265f * this.buildScale, 0f);
      localB.origin.set(0f, -0.225f * this.buildScale, 0f);
      joint6DOF = new Generic6DofConstraint(
          bodies[BodyPart.BODYPART_RIGHT_UPPER_LEG.ordinal()],
          bodies[BodyPart.BODYPART_RIGHT_LOWER_LEG.ordinal()],
          localA, localB, useLinearReferenceFrameA);
      //
      //#ifdef RIGID
      //joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
      //joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
      //#else
      tmp.set(-BulletGlobals.SIMD_EPSILON, -BulletGlobals.SIMD_EPSILON,
          -BulletGlobals.SIMD_PI * .9f);
      joint6DOF.setAngularLowerLimit(tmp);
      tmp.set(BulletGlobals.SIMD_EPSILON, BulletGlobals.SIMD_EPSILON,
          BulletGlobals.SIMD_EPSILON);
      joint6DOF.setAngularUpperLimit(tmp);
      //#endif
      joints[JointType.JOINT_RIGHT_KNEE.ordinal()] = joint6DOF;
      ownerWorld.addConstraint(
          joints[JointType.JOINT_RIGHT_KNEE.ordinal()], true);
    }

View Full Code Here


  public void freeze() {


    // Setup some damping on the m_bodies
    for (int i = 0; i < JointType.JOINT_COUNT.ordinal() - 1; ++i) {
      Generic6DofConstraint joint = (Generic6DofConstraint) joints[i];
      joint.setLimit(3, joint.getAngle(0), joint.getAngle(0));
      joint.setLimit(4, joint.getAngle(1), joint.getAngle(1));
      joint.setLimit(5, joint.getAngle(2), joint.getAngle(2));
      //joint.setLimit(3, joint.getAngle(3), joint.getAngle(3));
      //joint.setLimit(4, joint.getAngle(4), joint.getAngle(4));
      //joint.setLimit(5, joint.getAngle(5), joint.getAngle(5));
    }

View Full Code Here

  public float getScale() {
    return this.scale;
  }


  void liftLeg() {
    Generic6DofConstraint dofConstraint = (Generic6DofConstraint) joints[JointType.JOINT_RIGHT_KNEE
        .ordinal()];
    dofConstraint.buildJacobian();
    float angle = dofConstraint.getAngle(1);
    dofConstraint.testAngularLimitMotor(1);
    //  RotationalLimitMotor lim = dofConstraint.getRotationalLimitMotor(1);
    TranslationalLimitMotor translim = dofConstraint
        .getTranslationalLimitMotor();
    //dofConstraint.
    //  translim.
  }

View Full Code Here

      }
    }


    ///////////////////////////// SETTING THE CONSTRAINTS /////////////////////////////////////////////7777
    // Now setup the constraints
    Generic6DofConstraint joint6DOF;
    Transform localA = new Transform(), localB = new Transform();
    boolean useLinearReferenceFrameA = true;
    /// ******* SPINE HEAD ******** ///
    {
      localA.setIdentity();
      localB.setIdentity();


      localA.origin
          .set(0f,
              -(limbLengths[BodyPart.BODYPART_SPINE.ordinal()] + limbLengths[BodyPart.BODYPART_NECK
                  .ordinal()]), 0f);


      localB.origin.set(0f,
          limbLengths[BodyPart.BODYPART_HEAD.ordinal()], 0f);


      joint6DOF = new Generic6DofConstraint(
          bodies[BodyPart.BODYPART_SPINE.ordinal()],
          bodies[BodyPart.BODYPART_HEAD.ordinal()], localA, localB,
          useLinearReferenceFrameA);


      //#ifdef RIGID
      //joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
      //joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
      //#else


      tmp.set(-BulletGlobals.SIMD_PI * 0.2f, -BulletGlobals.FLT_EPSILON,
          -BulletGlobals.SIMD_PI * 0.1f);
      joint6DOF.setAngularLowerLimit(tmp);
      tmp.set(BulletGlobals.SIMD_PI * 0.1f, BulletGlobals.FLT_EPSILON,
          BulletGlobals.SIMD_PI * 0.10f);
      joint6DOF.setAngularUpperLimit(tmp);
      //#endif
      joints[JointType.JOINT_NECK_HEAD.ordinal()] = joint6DOF;
      ownerWorld.addConstraint(
          joints[JointType.JOINT_NECK_HEAD.ordinal()], true);


    }
    /// *************************** ///


    /// ******* PELVIS ******** ///
    {
      localA.setIdentity();
      localB.setIdentity();


      MatrixUtil.setEulerZYX(localA.basis, 0, BulletGlobals.SIMD_HALF_PI,
          0);
      localA.origin.set(0f,
          -limbLengths[BodyPart.BODYPART_PELVIS.ordinal()], 0f);
      MatrixUtil.setEulerZYX(localB.basis, 0, BulletGlobals.SIMD_HALF_PI,
          0);
      localB.origin.set(0f,
          limbLengths[BodyPart.BODYPART_SPINE.ordinal()], 0f);
      joint6DOF = new Generic6DofConstraint(
          bodies[BodyPart.BODYPART_PELVIS.ordinal()],
          bodies[BodyPart.BODYPART_SPINE.ordinal()], localA, localB,
          useLinearReferenceFrameA);


      //#ifdef RIGID
      //joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
      //joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
      //#else
      tmp.set(-BulletGlobals.SIMD_PI * 0.1f, -BulletGlobals.SIMD_EPSILON,
          -BulletGlobals.SIMD_PI * 0.1f);
      joint6DOF.setAngularLowerLimit(tmp);
      tmp.set(BulletGlobals.SIMD_PI * 0.2f, BulletGlobals.SIMD_EPSILON,
          BulletGlobals.SIMD_PI * 0.15f);
      joint6DOF.setAngularUpperLimit(tmp);
      //#endif
      joints[JointType.JOINT_PELVIS_SPINE.ordinal()] = joint6DOF;
      ownerWorld.addConstraint(
          joints[JointType.JOINT_PELVIS_SPINE.ordinal()], true);
    }
    /// *************************** ///


    /// ******* SPINE 2D ******** ///
    {
      localA.setIdentity();
      localB.setIdentity();


      localA.origin.set(0f, 0f, 0f);


      localB.origin.set(0f, 0f, 0f);


      CollisionShape ZeroShape = null;
      GLOBAL.jBullet.ZeroBody = new RigidBody(0.0f, null, ZeroShape);


      Transform Rotation = new Transform();
      Rotation.setIdentity();
      GLOBAL.jBullet.ZeroBody.setWorldTransform(Rotation);


      joint6DOF = new Generic6DofConstraint(
          bodies[BodyPart.BODYPART_SPINE.ordinal()],
          GLOBAL.jBullet.ZeroBody, localA, localB, false);


      joint6DOF.setLimit(0, 1, 0); // Disable X axis limits
      joint6DOF.setLimit(1, 1, 0); // Disable Y axis limits
      joint6DOF.setLimit(2, 0, 0); // Set the Z axis to always be equal to zero
      joint6DOF.setLimit(3, 0, 0); // Disable X rotational axes
      joint6DOF.setLimit(4, 0, 0); // Disable Y rotational axes
      joint6DOF.setLimit(5, 1, 0); // Uncap the rotational axes
      //#endif
      joints[JointType.JOINT_PLANE.ordinal()] = joint6DOF;
      ownerWorld.addConstraint(joints[JointType.JOINT_PLANE.ordinal()],
          true);


      bodies[BodyPart.BODYPART_SPINE.ordinal()]
          .setAngularFactor(new Vector3f(0, 0, 1));


    }
    /// *************************** ///


    if (hasArms) {


      /// ******* LEFT SHOULDER ******** ///
      {
        
        localA.setIdentity();
        localB.setIdentity();


        
        
        localA.origin.set(0f,
            -limbLengths[BodyPart.BODYPART_SPINE.ordinal()],
            limbWidths[BodyPart.BODYPART_SPINE.ordinal()] * 1.2f); //joint on spine
        MatrixUtil.setEulerZYX(localB.basis, BulletGlobals.SIMD_PI, BulletGlobals.SIMD_PI, BulletGlobals.SIMD_PI);
        localB.origin
            .set(0f, limbLengths[BodyPart.BODYPART_LEFT_UPPER_ARM
                .ordinal()], 0f); // upper arm


        joint6DOF = new Generic6DofConstraint(
            bodies[BodyPart.BODYPART_SPINE.ordinal()],
            bodies[BodyPart.BODYPART_LEFT_UPPER_ARM.ordinal()],
            localA, localB, useLinearReferenceFrameA);


            
        tmp.set(-BulletGlobals.SIMD_EPSILON, 
            -BulletGlobals.SIMD_EPSILON,
            BulletGlobals.SIMD_HALF_PI * 0.5f);
        joint6DOF.setAngularLowerLimit(tmp);
        tmp.set(BulletGlobals.SIMD_HALF_PI * 0.6f,
            BulletGlobals.SIMD_EPSILON,
            -BulletGlobals.SIMD_HALF_PI * 1.3f); //bend foward
        joint6DOF.setAngularUpperLimit(tmp);


        
        joints[JointType.JOINT_LEFT_SHOULDER.ordinal()] = joint6DOF;
        ownerWorld.addConstraint(
            joints[JointType.JOINT_LEFT_SHOULDER.ordinal()], true);
            
      }
      /// *************************** ///


      /// ******* RIGHT SHOULDER ******** ///
      {
        localA.setIdentity();
        localB.setIdentity();


        
        localA.origin.set(0f,
            -limbLengths[BodyPart.BODYPART_SPINE.ordinal()],
            -limbWidths[BodyPart.BODYPART_SPINE.ordinal()] * 1.2f); //joint on spine
        //MatrixUtil.setEulerZYX(localB.basis,BulletGlobals.SIMD_PI, BulletGlobals.SIMD_PI, BulletGlobals.SIMD_PI);
        localB.origin
            .set(0f, limbLengths[BodyPart.BODYPART_RIGHT_UPPER_ARM
                .ordinal()], 0f); // upper arm
        joint6DOF = new Generic6DofConstraint(
            bodies[BodyPart.BODYPART_SPINE.ordinal()],
            bodies[BodyPart.BODYPART_RIGHT_UPPER_ARM.ordinal()],
            localA, localB, useLinearReferenceFrameA);


        //#ifdef RIGID
        //joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
        //joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
        //#else


        tmp.set(-BulletGlobals.SIMD_EPSILON, 
            -BulletGlobals.SIMD_EPSILON,
            BulletGlobals.SIMD_HALF_PI * 0.5f);
        joint6DOF.setAngularLowerLimit(tmp);
        tmp.set(BulletGlobals.SIMD_HALF_PI * 0.6f,
            BulletGlobals.SIMD_EPSILON,
            -BulletGlobals.SIMD_HALF_PI * 1.3f); //bend foward
        joint6DOF.setAngularUpperLimit(tmp);


        //#endif
        joints[JointType.JOINT_RIGHT_SHOULDER.ordinal()] = joint6DOF;
        ownerWorld.addConstraint(
            joints[JointType.JOINT_RIGHT_SHOULDER.ordinal()], true);
      }
      /// *************************** ///


      
      /// ******* LEFT ELBOW ******** ///
      {
        localA.setIdentity();
        localB.setIdentity();


        localA.origin
            .set(0f, -limbLengths[BodyPart.BODYPART_LEFT_UPPER_ARM
                .ordinal()], 0f); // upper arm
        localB.origin
            .set(0f, -limbLengths[BodyPart.BODYPART_LEFT_LOWER_ARM
                .ordinal()], 0f); // lower arm
        joint6DOF = new Generic6DofConstraint(
            bodies[BodyPart.BODYPART_LEFT_UPPER_ARM.ordinal()],
            bodies[BodyPart.BODYPART_LEFT_LOWER_ARM.ordinal()],
            localA, localB, useLinearReferenceFrameA);


        //#ifdef RIGID
        //joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
        //joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
        //#else
        tmp.set(-BulletGlobals.SIMD_EPSILON, 
            -BulletGlobals.SIMD_EPSILON,
            -BulletGlobals.SIMD_HALF_PI * 0.5f);
        joint6DOF.setAngularLowerLimit(tmp);
        tmp.set(BulletGlobals.SIMD_HALF_PI * 0.6f,
            BulletGlobals.SIMD_EPSILON,
            BulletGlobals.SIMD_HALF_PI * 1.3f); //bend foward
        joint6DOF.setAngularUpperLimit(tmp);
        //#endif
        joints[JointType.JOINT_LEFT_ELBOW.ordinal()] = joint6DOF;
        ownerWorld.addConstraint(
            joints[JointType.JOINT_LEFT_ELBOW.ordinal()], true);
      }
      /// *************************** ///


      /// ******* RIGHT ELBOW ******** ///
      {
        localA.setIdentity();
        localB.setIdentity();


        localA.origin
            .set(0f, -limbLengths[BodyPart.BODYPART_LEFT_UPPER_ARM
                .ordinal()], 0f); // upper arm
        localB.origin
            .set(0f, -limbLengths[BodyPart.BODYPART_LEFT_LOWER_ARM
                .ordinal()], 0f); // lower arm
        joint6DOF = new Generic6DofConstraint(
            bodies[BodyPart.BODYPART_RIGHT_UPPER_ARM.ordinal()],
            bodies[BodyPart.BODYPART_RIGHT_LOWER_ARM.ordinal()],
            localA, localB, useLinearReferenceFrameA);


        //#ifdef RIGID
        //joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
        //joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
        //#else
        tmp.set(-BulletGlobals.SIMD_EPSILON, 
            -BulletGlobals.SIMD_EPSILON,
            -BulletGlobals.SIMD_HALF_PI * 0.5f);
        joint6DOF.setAngularLowerLimit(tmp);
        tmp.set(BulletGlobals.SIMD_HALF_PI * 0.6f,
            BulletGlobals.SIMD_EPSILON,
            BulletGlobals.SIMD_HALF_PI * 1.3f); //bend foward
        joint6DOF.setAngularUpperLimit(tmp);
        //#endif


        joints[JointType.JOINT_RIGHT_ELBOW.ordinal()] = joint6DOF;
        ownerWorld.addConstraint(
            joints[JointType.JOINT_RIGHT_ELBOW.ordinal()], true);
      }
      /// *************************** ///


      /// ******* RIGHT Wrist ******** ///
      {
        localA.setIdentity();
        localB.setIdentity();


        localA.origin
            .set(0f, limbLengths[BodyPart.BODYPART_RIGHT_LOWER_ARM
                .ordinal()], 0f); // upper arm
        localB.origin.set(0f,
            -limbLengths[BodyPart.BODYPART_RIGHT_HAND.ordinal()],
            0f); // lower arm
        joint6DOF = new Generic6DofConstraint(
            bodies[BodyPart.BODYPART_RIGHT_LOWER_ARM.ordinal()],
            bodies[BodyPart.BODYPART_RIGHT_HAND.ordinal()], localA,
            localB, useLinearReferenceFrameA);


        //#ifdef RIGID
        //joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
        //joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
        //#else
        tmp.set(-BulletGlobals.SIMD_EPSILON,
            -BulletGlobals.SIMD_EPSILON,
            -BulletGlobals.SIMD_EPSILON);
        joint6DOF.setAngularLowerLimit(tmp);
        tmp.set(BulletGlobals.SIMD_PI * 0.7f,
            BulletGlobals.SIMD_EPSILON, BulletGlobals.SIMD_EPSILON);
        joint6DOF.setAngularUpperLimit(tmp);
        //#endif


        joints[JointType.JOINT_RIGHT_WRIST.ordinal()] = joint6DOF;
        ownerWorld.addConstraint(
            joints[JointType.JOINT_RIGHT_WRIST.ordinal()], true);
      }
      /// *************************** ///


      /// ******* LEFT Wrist ******** ///
      {
        localA.setIdentity();
        localB.setIdentity();


        localA.origin
            .set(0f, limbLengths[BodyPart.BODYPART_LEFT_LOWER_ARM
                .ordinal()], 0f); // upper arm
        localB.origin
            .set(0f, -limbLengths[BodyPart.BODYPART_LEFT_HAND
                .ordinal()], 0f); // lower arm
        joint6DOF = new Generic6DofConstraint(
            bodies[BodyPart.BODYPART_LEFT_LOWER_ARM.ordinal()],
            bodies[BodyPart.BODYPART_LEFT_HAND.ordinal()], localA,
            localB, useLinearReferenceFrameA);


        //#ifdef RIGID
        //joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
        //joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
        //#else
        tmp.set(-BulletGlobals.SIMD_EPSILON,
            -BulletGlobals.SIMD_EPSILON,
            -BulletGlobals.SIMD_EPSILON);
        joint6DOF.setAngularLowerLimit(tmp);
        tmp.set(BulletGlobals.SIMD_PI * 0.7f,
            BulletGlobals.SIMD_EPSILON, BulletGlobals.SIMD_EPSILON);
        joint6DOF.setAngularUpperLimit(tmp);
        //#endif


        joints[JointType.JOINT_LEFT_WRIST.ordinal()] = joint6DOF;
        ownerWorld.addConstraint(
            joints[JointType.JOINT_LEFT_WRIST.ordinal()], true);
      }
      /// *************************** ///
       
       
    }


    /// ******* LEFT HIP ******** ///
    {
      localA.setIdentity();
      localB.setIdentity();


      localA.origin.set(0f,
          limbLengths[BodyPart.BODYPART_PELVIS.ordinal()],
          limbWidths[BodyPart.BODYPART_PELVIS.ordinal()]);


      localB.origin
          .set(0f, -limbLengths[BodyPart.BODYPART_LEFT_UPPER_LEG
              .ordinal()] * .6f, 0f);


      joint6DOF = new Generic6DofConstraint(
          bodies[BodyPart.BODYPART_PELVIS.ordinal()],
          bodies[BodyPart.BODYPART_LEFT_UPPER_LEG.ordinal()], localA,
          localB, useLinearReferenceFrameA);


      //#ifdef RIGID
      //joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
      //joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
      //#else
      tmp.set(-BulletGlobals.SIMD_EPSILON, 
          -BulletGlobals.SIMD_EPSILON,
          -BulletGlobals.SIMD_HALF_PI * 0.5f);
      joint6DOF.setAngularLowerLimit(tmp);
      tmp.set(BulletGlobals.SIMD_HALF_PI * 0.6f,
          BulletGlobals.SIMD_EPSILON,
          BulletGlobals.SIMD_HALF_PI * 1.3f); //bend foward
      joint6DOF.setAngularUpperLimit(tmp);
      //#endif
      joints[JointType.JOINT_LEFT_HIP.ordinal()] = joint6DOF;
      ownerWorld.addConstraint(
          joints[JointType.JOINT_LEFT_HIP.ordinal()], true);
    }
    /// *************************** ///


    /// ******* RIGHT HIP ******** ///
    {
      localA.setIdentity();
      localB.setIdentity();


      localA.origin.set(0f,
          limbLengths[BodyPart.BODYPART_PELVIS.ordinal()],
          -limbWidths[BodyPart.BODYPART_PELVIS.ordinal()]);


      localB.origin
          .set(0f, -limbLengths[BodyPart.BODYPART_RIGHT_UPPER_LEG
              .ordinal()] * .6f, 0f);


      joint6DOF = new Generic6DofConstraint(
          bodies[BodyPart.BODYPART_PELVIS.ordinal()],
          bodies[BodyPart.BODYPART_RIGHT_UPPER_LEG.ordinal()],
          localA, localB, useLinearReferenceFrameA);


      //#ifdef RIGID
      //joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
      //joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
      //#else
      tmp.set(-BulletGlobals.SIMD_HALF_PI * 0.6f,
          -BulletGlobals.SIMD_EPSILON,
          -BulletGlobals.SIMD_HALF_PI * 0.5f);
      joint6DOF.setAngularLowerLimit(tmp);
      tmp.set(BulletGlobals.SIMD_EPSILON, BulletGlobals.SIMD_EPSILON,
          BulletGlobals.SIMD_HALF_PI * 1.3f);
      joint6DOF.setAngularUpperLimit(tmp);
      //#endif
      joints[JointType.JOINT_RIGHT_HIP.ordinal()] = joint6DOF;
      ownerWorld.addConstraint(
          joints[JointType.JOINT_RIGHT_HIP.ordinal()], true);
    }
    /// *************************** ///


    /// ******* LEFT KNEE ******** ///
    {
      localA.setIdentity();
      localB.setIdentity();


      localA.origin
          .set(0f, limbLengths[BodyPart.BODYPART_LEFT_UPPER_LEG
              .ordinal()], 0f);
      localB.origin.set(0f,
          -limbLengths[BodyPart.BODYPART_LEFT_LOWER_LEG.ordinal()],
          0f);
      joint6DOF = new Generic6DofConstraint(
          bodies[BodyPart.BODYPART_LEFT_UPPER_LEG.ordinal()],
          bodies[BodyPart.BODYPART_LEFT_LOWER_LEG.ordinal()], localA,
          localB, useLinearReferenceFrameA);
      //
      //#ifdef RIGID
      //joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
      //joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
      //#else
      tmp.set(-BulletGlobals.SIMD_EPSILON, -BulletGlobals.SIMD_EPSILON,
          -BulletGlobals.SIMD_PI * .9f);
      joint6DOF.setAngularLowerLimit(tmp);
      tmp.set(BulletGlobals.SIMD_EPSILON, BulletGlobals.SIMD_EPSILON,
          BulletGlobals.SIMD_EPSILON);
      joint6DOF.setAngularUpperLimit(tmp);
      //#endif
      joints[JointType.JOINT_LEFT_KNEE.ordinal()] = joint6DOF;
      ownerWorld.addConstraint(
          joints[JointType.JOINT_LEFT_KNEE.ordinal()], true);
    }
    /// *************************** ///


    /// ******* RIGHT KNEE ******** ///
    {
      localA.setIdentity();
      localB.setIdentity();
      HingeConstraint jointHinge = null;
      localA.origin.set(0f,
          limbLengths[BodyPart.BODYPART_RIGHT_UPPER_LEG.ordinal()],
          0f);
      localB.origin.set(0f,
          -limbLengths[BodyPart.BODYPART_RIGHT_LOWER_LEG.ordinal()],
          0f);
      joint6DOF = new Generic6DofConstraint(
          bodies[BodyPart.BODYPART_RIGHT_UPPER_LEG.ordinal()],
          bodies[BodyPart.BODYPART_RIGHT_LOWER_LEG.ordinal()],
          localA, localB, useLinearReferenceFrameA);
      //
      //#ifdef RIGID
      //joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
      //joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
      //#else
      tmp.set(-BulletGlobals.SIMD_EPSILON, -BulletGlobals.SIMD_EPSILON,
          -BulletGlobals.SIMD_PI * .9f);
      joint6DOF.setAngularLowerLimit(tmp);
      tmp.set(BulletGlobals.SIMD_EPSILON, BulletGlobals.SIMD_EPSILON,
          BulletGlobals.SIMD_EPSILON);
      joint6DOF.setAngularUpperLimit(tmp);
      //#endif
      joints[JointType.JOINT_RIGHT_KNEE.ordinal()] = joint6DOF;
      ownerWorld.addConstraint(
          joints[JointType.JOINT_RIGHT_KNEE.ordinal()], true);
    }
    /// *************************** ///


    /// ******* RIGHT FOOT ******** ///
    {
      localA.setIdentity();
      localB.setIdentity();
      HingeConstraint jointHinge = null;
      localA.origin.set(0f,
          limbLengths[BodyPart.BODYPART_RIGHT_LOWER_LEG.ordinal()],
          0f);
      localB.origin.set(0f,
          -limbLengths[BodyPart.BODYPART_RIGHT_FOOT.ordinal()] * .5f,
          0f);
      joint6DOF = new Generic6DofConstraint(
          bodies[BodyPart.BODYPART_RIGHT_LOWER_LEG.ordinal()],
          bodies[BodyPart.BODYPART_RIGHT_FOOT.ordinal()], localA,
          localB, useLinearReferenceFrameA);
      //
      //#ifdef RIGID
      //joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
      //joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
      //#else
      tmp.set(-BulletGlobals.SIMD_EPSILON, -BulletGlobals.SIMD_EPSILON,
          BulletGlobals.SIMD_EPSILON);
      joint6DOF.setAngularLowerLimit(tmp);
      tmp.set(BulletGlobals.SIMD_EPSILON, BulletGlobals.SIMD_EPSILON,
          BulletGlobals.SIMD_PI * .7f);
      joint6DOF.setAngularUpperLimit(tmp);
      //#endif
      joints[JointType.JOINT_RIGHT_ANKLE.ordinal()] = joint6DOF;
      ownerWorld.addConstraint(
          joints[JointType.JOINT_RIGHT_ANKLE.ordinal()], true);
    }
    /// *************************** ///


    /// ******* LEFT FOOT ******** ///
    {
      localA.setIdentity();
      localB.setIdentity();
      HingeConstraint jointHinge = null;
      localA.origin
          .set(0f, limbLengths[BodyPart.BODYPART_LEFT_LOWER_LEG
              .ordinal()], 0f);
      localB.origin.set(0f,
          -limbLengths[BodyPart.BODYPART_LEFT_FOOT.ordinal()] * .5f,
          0f);
      joint6DOF = new Generic6DofConstraint(
          bodies[BodyPart.BODYPART_LEFT_LOWER_LEG.ordinal()],
          bodies[BodyPart.BODYPART_LEFT_FOOT.ordinal()], localA,
          localB, useLinearReferenceFrameA);
      //
      //#ifdef RIGID
      //joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
      //joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
      //#else
      tmp.set(-BulletGlobals.SIMD_EPSILON, -BulletGlobals.SIMD_EPSILON,
          BulletGlobals.SIMD_EPSILON);
      joint6DOF.setAngularLowerLimit(tmp);
      tmp.set(BulletGlobals.SIMD_EPSILON, BulletGlobals.SIMD_EPSILON,
          BulletGlobals.SIMD_PI * .7f);
      joint6DOF.setAngularUpperLimit(tmp);
      //#endif
      joints[JointType.JOINT_LEFT_ANKLE.ordinal()] = joint6DOF;
      ownerWorld.addConstraint(
          joints[JointType.JOINT_LEFT_ANKLE.ordinal()], true);
    }

View Full Code Here

  public void freeze() {


    // Setup some damping on the m_bodies
    for (int i = 1; i < JointType.JOINT_COUNT.ordinal(); ++i) {
      if (joints[i] != null) {
        Generic6DofConstraint joint = (Generic6DofConstraint) joints[i];
        joint.setLimit(3, joint.getAngle(0), joint.getAngle(0));
        joint.setLimit(4, joint.getAngle(1), joint.getAngle(1));
        joint.setLimit(5, joint.getAngle(2), joint.getAngle(2));
        //joint.setLimit(3, joint.getAngle(3), joint.getAngle(3));
        //joint.setLimit(4, joint.getAngle(4), joint.getAngle(4));
        //joint.setLimit(5, joint.getAngle(5), joint.getAngle(5));
      }
    }

View Full Code Here

    this.noCollisions();


  }


  void liftLeg() {
    Generic6DofConstraint dofConstraint = (Generic6DofConstraint) joints[JointType.JOINT_RIGHT_KNEE
        .ordinal()];
    dofConstraint.buildJacobian();
    float angle = dofConstraint.getAngle(1);
    dofConstraint.testAngularLimitMotor(1);
    //  RotationalLimitMotor lim = dofConstraint.getRotationalLimitMotor(1);
    TranslationalLimitMotor translim = dofConstraint
        .getTranslationalLimitMotor();
    //dofConstraint.
    //  translim.
  }

View Full Code Here

    Transform localB = new Transform();
    localB.setIdentity();
    
    localA.origin.set(new Vector3f (0,0,0));
    localB.origin.set(new Vector3f (0,0,0));
    Generic6DofConstraint senkrecht = new Generic6DofConstraint(
        groundAgent.getRigidBody(), 
        gewichtAgent.getRigidBody(), 
        localA, 
        localB, 
        true);
    
    senkrecht.setLimit(0, -.9f, .9f);
    senkrecht.setLimit(1, 7, 100);
    senkrecht.setLimit(2, -.9f, .9f);


    env.getDynamicsWorld().addConstraint(gelenk1, true);
    env.getDynamicsWorld().addConstraint(gelenk2, true);
    env.getDynamicsWorld().addConstraint(gewichtBefestigung, true);
    env.getDynamicsWorld().addConstraint(senkrecht, true);
    
    env.addAgent(groundAgent);
    env.addAgent(armAgent1);
    env.addAgent(armAgent2);
    env.addAgent(gewichtAgent);
    
    
    
    
    ///////////////////////////////////////
    ////////////PENDEL
    ///////////////////////////////////////
    BoxShape pendelShape = new BoxShape (new Vector3f (.5f,10,.5f));
    env.getCollisionShapes().add(pendelShape);
    
    ConstraintDemoAgent pendelAgent = new ConstraintDemoAgent(
        idCounter++, 
        env, 
        env.getCollisionShapes().getQuick(2), 
        new Vector3f (10,25,10), 
        null, 
        .5f);
    
    ConstraintDemoAgent pendelKugelAgent = new ConstraintDemoAgent(
        idCounter++, 
        env, 
        env.getCollisionShapes().getQuick(1), 
        new Vector3f (10,5,10), 
        null, 
        5f);
    
    Point2PointConstraint pendelAufhaengung = new Point2PointConstraint(
        pendelAgent.getRigidBody(), 
        new Vector3f (0,11,0));
    
    Point2PointConstraint pendelKugelBefestigung = new Point2PointConstraint(
        pendelAgent.getRigidBody(), 
        pendelKugelAgent.getRigidBody(), 
        new Vector3f (0,-10,0), 
        new Vector3f (0,0,0));
    
    pendelKugelAgent.getRigidBody().setActivationState(CollisionObject.DISABLE_DEACTIVATION);
    pendelAgent.getRigidBody().setActivationState(CollisionObject.DISABLE_DEACTIVATION);
    
    env.addAgent(pendelAgent);
    env.addAgent(pendelKugelAgent);
    env.getDynamicsWorld().addConstraint(pendelAufhaengung, true);
    env.getDynamicsWorld().addConstraint(pendelKugelBefestigung, true);
    
    
    
    //////////////////////////////////
    ////////////// DOPSBALL
    //////////////////////////////////
    
    ConstraintDemoAgent dopsBall = new ConstraintDemoAgent(
        idCounter++,
        env, 
        env.getCollisionShapes().getQuick(1), 
        new Vector3f (-20,40,20), 
        null, 
        1);
    
    dopsBall.getRigidBody().setActivationState(CollisionObject.DISABLE_DEACTIVATION);
    dopsBall.getRigidBody().setRestitution(.8f);
    groundAgent.getRigidBody().setRestitution(.8f);
    
    
    localA.setIdentity();
    localB.setIdentity();
    localA.origin.set (new Vector3f (-20,0,20));
    localB.origin.set (new Vector3f (0,0,0));
    
    Generic6DofConstraint dopsBallConstraint = new Generic6DofConstraint(
        groundAgent.getRigidBody(), 
        dopsBall.getRigidBody(), 
        localA, 
        localB, 
        true);
    
    dopsBallConstraint.setLimit(1, 1, 100);
    
    env.addAgent(dopsBall);
    env.getDynamicsWorld().addConstraint(dopsBallConstraint, false);


    
    
    
    ////////////////////////////////////
    ///// SCHWEBENDER BALL
    ////////////////////////////////
    
    ConstraintDemoAgent schwebenderBallAgent = new ConstraintDemoAgent(
        idCounter++,
        env,
        env.getCollisionShapes().getQuick(1),
        new Vector3f (0,10,30),
        null,
        1);
    
    schwebenderBallAgent.getRigidBody().setActivationState(CollisionObject.DISABLE_DEACTIVATION);
    schwebenderBallAgent.getRigidBody().setRestitution(1);
    schwebenderBallAgent.getRigidBody().setFriction(0);
    schwebenderBallAgent.getRigidBody().setGravity(new Vector3f(0,0,0));
    
    localA.setIdentity();
    localB.setIdentity();
    localA.origin.set(new Vector3f (0,10,30));
    localB.origin.set(new Vector3f (0,0,0));
    
    Generic6DofConstraint schwebenderBallAgentConstraint = new Generic6DofConstraint(
        groundAgent.getRigidBody(), 
        schwebenderBallAgent.getRigidBody(), 
        localA, 
        localB, 
        true);
    
    schwebenderBallAgentConstraint.setLimit(0, -30, 30);
    
    env.addAgent(schwebenderBallAgent);
    env.getDynamicsWorld().addConstraint(schwebenderBallAgentConstraint, false);
        
    return new ConstraintDemoEnvironment[] {env};

View Full Code Here

        -positionOnPredecessor.y * this.getSize().y,
        -positionOnPredecessor.z * this.getSize().z
        ));
    
    // Create actual constraint.
    g6dofc = new Generic6DofConstraint(
        predecessor.getRigidBody(), 
        this.getRigidBody(), 
        frameInPredecessor, 
        frameInThis, 
        false);

View Full Code Here

      bodies[i].setSleepingThresholds(1.6f, 2.5f);
    }


    ///////////////////////////// SETTING THE CONSTRAINTS /////////////////////////////////////////////7777
    // Now setup the constraints
    Generic6DofConstraint joint6DOF;
    Transform localA = new Transform(), localB = new Transform();
    boolean useLinearReferenceFrameA = true;
    /// ******* SPINE HEAD ******** ///
    {
      localA.setIdentity();
      localB.setIdentity();


      localA.origin.set(0f, 0.30f * scale_ragdoll, 0f);


      localB.origin.set(0f, -0.14f * scale_ragdoll, 0f);


      joint6DOF = new Generic6DofConstraint(bodies[BodyPart.BODYPART_SPINE.ordinal()], bodies[BodyPart.BODYPART_HEAD.ordinal()], localA, localB, useLinearReferenceFrameA);


      //#ifdef RIGID
      //joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
      //joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
      //#else
      tmp.set(-BulletGlobals.SIMD_PI * 0.3f, -BulletGlobals.FLT_EPSILON, -BulletGlobals.SIMD_PI * 0.3f);
      joint6DOF.setAngularLowerLimit(tmp);
      tmp.set(BulletGlobals.SIMD_PI * 0.5f, BulletGlobals.FLT_EPSILON, BulletGlobals.SIMD_PI * 0.3f);
      joint6DOF.setAngularUpperLimit(tmp);
      //#endif
      joints[JointType.JOINT_SPINE_HEAD.ordinal()] = joint6DOF;
      ownerWorld.addConstraint(joints[JointType.JOINT_SPINE_HEAD.ordinal()], true);
    }
    /// *************************** ///


    /// ******* LEFT SHOULDER ******** ///
    {
      localA.setIdentity();
      localB.setIdentity();


      localA.origin.set(-0.2f * scale_ragdoll, 0.15f * scale_ragdoll, 0f);


      MatrixUtil.setEulerZYX(localB.basis, BulletGlobals.SIMD_HALF_PI, 0, -BulletGlobals.SIMD_HALF_PI);
      localB.origin.set(0f, -0.18f * scale_ragdoll, 0f);


      joint6DOF = new Generic6DofConstraint(bodies[BodyPart.BODYPART_SPINE.ordinal()], bodies[BodyPart.BODYPART_LEFT_UPPER_ARM.ordinal()], localA, localB, useLinearReferenceFrameA);


      //#ifdef RIGID
      //joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
      //joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
      //#else
      tmp.set(-BulletGlobals.SIMD_PI * 0.8f, -BulletGlobals.FLT_EPSILON, -BulletGlobals.SIMD_PI * 0.5f);
      joint6DOF.setAngularLowerLimit(tmp);
      tmp.set(BulletGlobals.SIMD_PI * 0.8f, BulletGlobals.FLT_EPSILON, BulletGlobals.SIMD_PI * 0.5f);
      joint6DOF.setAngularUpperLimit(tmp);
      //#endif
      joints[JointType.JOINT_LEFT_SHOULDER.ordinal()] = joint6DOF;
      ownerWorld.addConstraint(joints[JointType.JOINT_LEFT_SHOULDER.ordinal()], true);
    }
    /// *************************** ///


    /// ******* RIGHT SHOULDER ******** ///
    {
      localA.setIdentity();
      localB.setIdentity();


      localA.origin.set(0.2f * scale_ragdoll, 0.15f * scale_ragdoll, 0f);
      MatrixUtil.setEulerZYX(localB.basis, 0, 0, BulletGlobals.SIMD_HALF_PI);
      localB.origin.set(0f, -0.18f * scale_ragdoll, 0f);
      joint6DOF = new Generic6DofConstraint(bodies[BodyPart.BODYPART_SPINE.ordinal()], bodies[BodyPart.BODYPART_RIGHT_UPPER_ARM.ordinal()], localA, localB, useLinearReferenceFrameA);


      //#ifdef RIGID
      //joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
      //joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
      //#else
      tmp.set(-BulletGlobals.SIMD_PI * 0.8f, -BulletGlobals.SIMD_EPSILON, -BulletGlobals.SIMD_PI * 0.5f);
      joint6DOF.setAngularLowerLimit(tmp);
      tmp.set(BulletGlobals.SIMD_PI * 0.8f, BulletGlobals.SIMD_EPSILON, BulletGlobals.SIMD_PI * 0.5f);
      joint6DOF.setAngularUpperLimit(tmp);
      //#endif
      joints[JointType.JOINT_RIGHT_SHOULDER.ordinal()] = joint6DOF;
      ownerWorld.addConstraint(joints[JointType.JOINT_RIGHT_SHOULDER.ordinal()], true);
    }
    /// *************************** ///


    /// ******* LEFT ELBOW ******** ///
    {
      localA.setIdentity();
      localB.setIdentity();


      localA.origin.set(0f, 0.18f * scale_ragdoll, 0f);
      localB.origin.set(0f, -0.14f * scale_ragdoll, 0f);
      joint6DOF = new Generic6DofConstraint(bodies[BodyPart.BODYPART_LEFT_UPPER_ARM.ordinal()], bodies[BodyPart.BODYPART_LEFT_LOWER_ARM.ordinal()], localA, localB, useLinearReferenceFrameA);


      //#ifdef RIGID
      //joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
      //joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
      //#else
      tmp.set(-BulletGlobals.SIMD_EPSILON, -BulletGlobals.SIMD_EPSILON, -BulletGlobals.SIMD_EPSILON);
      joint6DOF.setAngularLowerLimit(tmp);
      tmp.set(BulletGlobals.SIMD_PI * 0.7f, BulletGlobals.SIMD_EPSILON, BulletGlobals.SIMD_EPSILON);
      joint6DOF.setAngularUpperLimit(tmp);
      //#endif
      joints[JointType.JOINT_LEFT_ELBOW.ordinal()] = joint6DOF;
      ownerWorld.addConstraint(joints[JointType.JOINT_LEFT_ELBOW.ordinal()], true);
    }
    /// *************************** ///


    /// ******* RIGHT ELBOW ******** ///
    {
      localA.setIdentity();
      localB.setIdentity();


      localA.origin.set(0f, 0.18f * scale_ragdoll, 0f);
      localB.origin.set(0f, -0.14f * scale_ragdoll, 0f);
      joint6DOF = new Generic6DofConstraint(bodies[BodyPart.BODYPART_RIGHT_UPPER_ARM.ordinal()], bodies[BodyPart.BODYPART_RIGHT_LOWER_ARM.ordinal()], localA, localB, useLinearReferenceFrameA);


      //#ifdef RIGID
      //joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
      //joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
      //#else
      tmp.set(-BulletGlobals.SIMD_EPSILON, -BulletGlobals.SIMD_EPSILON, -BulletGlobals.SIMD_EPSILON);
      joint6DOF.setAngularLowerLimit(tmp);
      tmp.set(BulletGlobals.SIMD_PI * 0.7f, BulletGlobals.SIMD_EPSILON, BulletGlobals.SIMD_EPSILON);
      joint6DOF.setAngularUpperLimit(tmp);
      //#endif


      joints[JointType.JOINT_RIGHT_ELBOW.ordinal()] = joint6DOF;
      ownerWorld.addConstraint(joints[JointType.JOINT_RIGHT_ELBOW.ordinal()], true);
    }
    /// *************************** ///




    /// ******* PELVIS ******** ///
    {
      localA.setIdentity();
      localB.setIdentity();


      MatrixUtil.setEulerZYX(localA.basis, 0, BulletGlobals.SIMD_HALF_PI, 0);
      localA.origin.set(0f, 0.15f * scale_ragdoll, 0f);
      MatrixUtil.setEulerZYX(localB.basis, 0, BulletGlobals.SIMD_HALF_PI, 0);
      localB.origin.set(0f, -0.15f * scale_ragdoll, 0f);
      joint6DOF = new Generic6DofConstraint(bodies[BodyPart.BODYPART_PELVIS.ordinal()], bodies[BodyPart.BODYPART_SPINE.ordinal()], localA, localB, useLinearReferenceFrameA);


      //#ifdef RIGID
      //joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
      //joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
      //#else
      tmp.set(-BulletGlobals.SIMD_PI * 0.2f, -BulletGlobals.SIMD_EPSILON, -BulletGlobals.SIMD_PI * 0.3f);
      joint6DOF.setAngularLowerLimit(tmp);
      tmp.set(BulletGlobals.SIMD_PI * 0.2f, BulletGlobals.SIMD_EPSILON, BulletGlobals.SIMD_PI * 0.6f);
      joint6DOF.setAngularUpperLimit(tmp);
      //#endif
      joints[JointType.JOINT_PELVIS_SPINE.ordinal()] = joint6DOF;
      ownerWorld.addConstraint(joints[JointType.JOINT_PELVIS_SPINE.ordinal()], true);
    }
    /// *************************** ///


    /// ******* LEFT HIP ******** ///
    {
      localA.setIdentity();
      localB.setIdentity();


      localA.origin.set(-0.18f * scale_ragdoll, -0.10f * scale_ragdoll, 0f);


      localB.origin.set(0f, 0.225f * scale_ragdoll, 0f);


      joint6DOF = new Generic6DofConstraint(bodies[BodyPart.BODYPART_PELVIS.ordinal()], bodies[BodyPart.BODYPART_LEFT_UPPER_LEG.ordinal()], localA, localB, useLinearReferenceFrameA);


      //#ifdef RIGID
      //joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
      //joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
      //#else
      tmp.set(-BulletGlobals.SIMD_HALF_PI * 0.5f, -BulletGlobals.SIMD_EPSILON, -BulletGlobals.SIMD_EPSILON);
      joint6DOF.setAngularLowerLimit(tmp);
      tmp.set(BulletGlobals.SIMD_HALF_PI * 0.8f, BulletGlobals.SIMD_EPSILON, BulletGlobals.SIMD_HALF_PI * 0.6f);
      joint6DOF.setAngularUpperLimit(tmp);
      //#endif
      joints[JointType.JOINT_LEFT_HIP.ordinal()] = joint6DOF;
      ownerWorld.addConstraint(joints[JointType.JOINT_LEFT_HIP.ordinal()], true);
    }
    /// *************************** ///




    /// ******* RIGHT HIP ******** ///
    {
      localA.setIdentity();
      localB.setIdentity();


      localA.origin.set(0.18f * scale_ragdoll, -0.10f * scale_ragdoll, 0f);
      localB.origin.set(0f, 0.225f * scale_ragdoll, 0f);


      joint6DOF = new Generic6DofConstraint(bodies[BodyPart.BODYPART_PELVIS.ordinal()], bodies[BodyPart.BODYPART_RIGHT_UPPER_LEG.ordinal()], localA, localB, useLinearReferenceFrameA);


      //#ifdef RIGID
      //joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
      //joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
      //#else
      tmp.set(-BulletGlobals.SIMD_HALF_PI * 0.5f, -BulletGlobals.SIMD_EPSILON, -BulletGlobals.SIMD_HALF_PI * 0.6f);
      joint6DOF.setAngularLowerLimit(tmp);
      tmp.set(BulletGlobals.SIMD_HALF_PI * 0.8f, BulletGlobals.SIMD_EPSILON, BulletGlobals.SIMD_EPSILON);
      joint6DOF.setAngularUpperLimit(tmp);
      //#endif
      joints[JointType.JOINT_RIGHT_HIP.ordinal()] = joint6DOF;
      ownerWorld.addConstraint(joints[JointType.JOINT_RIGHT_HIP.ordinal()], true);
    }
    /// *************************** ///




    /// ******* LEFT KNEE ******** ///
    {
      localA.setIdentity();
      localB.setIdentity();


      localA.origin.set(0f, -0.225f * scale_ragdoll, 0f);
      localB.origin.set(0f, 0.185f * scale_ragdoll, 0f);
      joint6DOF = new Generic6DofConstraint(bodies[BodyPart.BODYPART_LEFT_UPPER_LEG.ordinal()], bodies[BodyPart.BODYPART_LEFT_LOWER_LEG.ordinal()], localA, localB, useLinearReferenceFrameA);
      //
      //#ifdef RIGID
      //joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
      //joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
      //#else
      tmp.set(-BulletGlobals.SIMD_EPSILON, -BulletGlobals.SIMD_EPSILON, -BulletGlobals.SIMD_EPSILON);
      joint6DOF.setAngularLowerLimit(tmp);
      tmp.set(BulletGlobals.SIMD_PI * 0.7f, BulletGlobals.SIMD_EPSILON, BulletGlobals.SIMD_EPSILON);
      joint6DOF.setAngularUpperLimit(tmp);
      //#endif
      joints[JointType.JOINT_LEFT_KNEE.ordinal()] = joint6DOF;
      ownerWorld.addConstraint(joints[JointType.JOINT_LEFT_KNEE.ordinal()], true);
    }
    /// *************************** ///


    /// ******* RIGHT KNEE ******** ///
    {
      localA.setIdentity();
      localB.setIdentity();


      localA.origin.set(0f, -0.225f * scale_ragdoll, 0f);
      localB.origin.set(0f, 0.185f * scale_ragdoll, 0f);
      joint6DOF = new Generic6DofConstraint(bodies[BodyPart.BODYPART_RIGHT_UPPER_LEG.ordinal()], bodies[BodyPart.BODYPART_RIGHT_LOWER_LEG.ordinal()], localA, localB, useLinearReferenceFrameA);


      //#ifdef RIGID
      //joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
      //joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
      //#else
      tmp.set(-BulletGlobals.SIMD_EPSILON, -BulletGlobals.SIMD_EPSILON, -BulletGlobals.SIMD_EPSILON);
      joint6DOF.setAngularLowerLimit(tmp);
      tmp.set(BulletGlobals.SIMD_PI * 0.7f, BulletGlobals.SIMD_EPSILON, BulletGlobals.SIMD_EPSILON);
      joint6DOF.setAngularUpperLimit(tmp);
      //#endif
      joints[JointType.JOINT_RIGHT_KNEE.ordinal()] = joint6DOF;
      ownerWorld.addConstraint(joints[JointType.JOINT_RIGHT_KNEE.ordinal()], true);
    }
    /// *************************** ///

View Full Code Here

TOP

Related Classes of com.bulletphysics.dynamics.constraintsolver.Generic6DofConstraint

cc.sketchchair.core.dollBackup

cc.sketchchair.ragdoll.ergoDoll

com.bulletphysics.demos.genericjoint.RagDoll

eas.users.students.fabian.diplomarbeit.EvolvableBoxAgents3D.Agents.EvolvableBoxAgent3DAppendix

eas.users.students.fabian.diplomarbeit.sim3D.physicalSimulation.ConstraintDemo.ConstraintDemoScheduler

javax.vecmath.Matrix3f

javax.vecmath.Vector3f

All source code are property of their respective owners. Java is a trademark of Sun Microsystems, Inc and owned by ORACLE Inc. Contact coftware#gmail.com.

AXIS	MIN ANGLE	MAX ANGLE
X	-PI	PI
Y	-PI/2	PI/2
Z	-PI/2	PI/2