Exemplo n.º 1
0
    public Vector3f GetCoordinates(Face face, Vector3f out) {
      Vector3f tmp = Stack.alloc(Vector3f.class);
      Vector3f tmp1 = Stack.alloc(Vector3f.class);
      Vector3f tmp2 = Stack.alloc(Vector3f.class);

      Vector3f o = Stack.alloc(Vector3f.class);
      o.scale(-face.d, face.n);

      float[] a = floatArrays.getFixed(3);

      tmp1.sub(face.v[0].w, o);
      tmp2.sub(face.v[1].w, o);
      tmp.cross(tmp1, tmp2);
      a[0] = tmp.length();

      tmp1.sub(face.v[1].w, o);
      tmp2.sub(face.v[2].w, o);
      tmp.cross(tmp1, tmp2);
      a[1] = tmp.length();

      tmp1.sub(face.v[2].w, o);
      tmp2.sub(face.v[0].w, o);
      tmp.cross(tmp1, tmp2);
      a[2] = tmp.length();

      float sm = a[0] + a[1] + a[2];

      out.set(a[1], a[2], a[0]);
      out.scale(1f / (sm > 0f ? sm : 1f));

      floatArrays.release(a);

      return out;
    }
Exemplo n.º 2
0
    public boolean Set(Face f, Mkv a, Mkv b, Mkv c) {
      Vector3f tmp1 = Stack.alloc(Vector3f.class);
      Vector3f tmp2 = Stack.alloc(Vector3f.class);
      Vector3f tmp3 = Stack.alloc(Vector3f.class);

      Vector3f nrm = Stack.alloc(Vector3f.class);
      tmp1.sub(b.w, a.w);
      tmp2.sub(c.w, a.w);
      nrm.cross(tmp1, tmp2);

      float len = nrm.length();

      tmp1.cross(a.w, b.w);
      tmp2.cross(b.w, c.w);
      tmp3.cross(c.w, a.w);

      boolean valid =
          (tmp1.dot(nrm) >= -EPA_inface_eps)
              && (tmp2.dot(nrm) >= -EPA_inface_eps)
              && (tmp3.dot(nrm) >= -EPA_inface_eps);

      f.v[0] = a;
      f.v[1] = b;
      f.v[2] = c;
      f.mark = 0;
      f.n.scale(1f / (len > 0f ? len : cstInf), nrm);
      f.d = Math.max(0, -f.n.dot(a.w));
      return valid;
    }
Exemplo n.º 3
0
  @Override
  public Vector3f localGetSupportingVertexWithoutMargin(Vector3f vec0, Vector3f out) {
    Vector3f supVec = out;
    supVec.set(0f, 0f, 0f);
    float newDot, maxDot = -1e30f;

    Vector3f vec = Stack.alloc(vec0);
    float lenSqr = vec.lengthSquared();
    if (lenSqr < 0.0001f) {
      vec.set(1f, 0f, 0f);
    } else {
      float rlen = 1f / (float) Math.sqrt(lenSqr);
      vec.scale(rlen);
    }

    Vector3f vtx = Stack.alloc(Vector3f.class);
    for (int i = 0; i < points.size(); i++) {
      VectorUtil.mul(vtx, points.getQuick(i), localScaling);

      newDot = vec.dot(vtx);
      if (newDot > maxDot) {
        maxDot = newDot;
        supVec.set(vtx);
      }
    }
    return out;
  }
Exemplo n.º 4
0
    public boolean SearchOrigin(Vector3f initray) {
      Vector3f tmp1 = Stack.alloc(Vector3f.class);
      Vector3f tmp2 = Stack.alloc(Vector3f.class);
      Vector3f tmp3 = Stack.alloc(Vector3f.class);
      Vector3f tmp4 = Stack.alloc(Vector3f.class);

      iterations = 0;
      order = -1;
      failed = false;
      ray.set(initray);
      ray.normalize();

      Arrays.fill(table, null);

      FetchSupport();
      ray.negate(simplex[0].w);
      for (; iterations < GJK_maxiterations; ++iterations) {
        float rl = ray.length();
        ray.scale(1f / (rl > 0f ? rl : 1f));
        if (FetchSupport()) {
          boolean found = false;
          switch (order) {
            case 1:
              {
                tmp1.negate(simplex[1].w);
                tmp2.sub(simplex[0].w, simplex[1].w);
                found = SolveSimplex2(tmp1, tmp2);
                break;
              }
            case 2:
              {
                tmp1.negate(simplex[2].w);
                tmp2.sub(simplex[1].w, simplex[2].w);
                tmp3.sub(simplex[0].w, simplex[2].w);
                found = SolveSimplex3(tmp1, tmp2, tmp3);
                break;
              }
            case 3:
              {
                tmp1.negate(simplex[3].w);
                tmp2.sub(simplex[2].w, simplex[3].w);
                tmp3.sub(simplex[1].w, simplex[3].w);
                tmp4.sub(simplex[0].w, simplex[3].w);
                found = SolveSimplex4(tmp1, tmp2, tmp3, tmp4);
                break;
              }
          }
          if (found) {
            return true;
          }
        } else {
          return false;
        }
      }
      failed = true;
      return false;
    }
  protected void integrateTransforms(float timeStep) {
    Vector3f tmp = Stack.alloc(Vector3f.class);
    Transform tmpTrans = Stack.alloc(Transform.class);

    Transform predictedTrans = Stack.alloc(Transform.class);
    for (int i = 0; i < collisionObjects.size(); i++) {
      CollisionObject colObj = collisionObjects.getQuick(i);
      RigidBody body = RigidBody.upcast(colObj);
      if (body != null) {
        body.setHitFraction(1f);

        if (body.isActive() && (!body.isStaticOrKinematicObject())) {
          body.predictIntegratedTransform(timeStep, predictedTrans);

          tmp.sub(predictedTrans.origin, body.getWorldTransform(tmpTrans).origin);
          float squareMotion = tmp.lengthSquared();

          if (body.getCcdSquareMotionThreshold() != 0f
              && body.getCcdSquareMotionThreshold() < squareMotion) {
            if (body.getCollisionShape().isConvex()) {
              ClosestNotMeConvexResultCallback sweepResults =
                  new ClosestNotMeConvexResultCallback(
                      body,
                      body.getWorldTransform(tmpTrans).origin,
                      predictedTrans.origin,
                      getBroadphase().getOverlappingPairCache(),
                      getDispatcher());
              // ConvexShape convexShape = (ConvexShape)body.getCollisionShape();
              SphereShape tmpSphere =
                  new SphereShape(
                      body
                          .getCcdSweptSphereRadius()); // btConvexShape* convexShape =
                                                       // static_cast<btConvexShape*>(body->getCollisionShape());

              sweepResults.collisionFilterGroup = body.getBroadphaseProxy().collisionFilterGroup;
              sweepResults.collisionFilterMask = body.getBroadphaseProxy().collisionFilterMask;

              convexSweepTest(
                  tmpSphere, body.getWorldTransform(tmpTrans), predictedTrans, sweepResults);
              // JAVA NOTE: added closestHitFraction test to prevent objects being stuck
              if (sweepResults.hasHit() && (sweepResults.closestHitFraction > 0.0001f)) {
                body.setHitFraction(sweepResults.closestHitFraction);
                body.predictIntegratedTransform(timeStep * body.getHitFraction(), predictedTrans);
                body.setHitFraction(0f);
                // System.out.printf("clamped integration to hit fraction = %f\n",
                // sweepResults.closestHitFraction);
              }
            }
          }

          body.proceedToTransform(predictedTrans);
        }
      }
    }
  }
Exemplo n.º 6
0
    public void Support(Vector3f d, Mkv v) {
      v.r.set(d);

      Vector3f tmp1 = LocalSupport(d, 0, Stack.alloc(Vector3f.class));

      Vector3f tmp = Stack.alloc(Vector3f.class);
      tmp.set(d);
      tmp.negate();
      Vector3f tmp2 = LocalSupport(tmp, 1, Stack.alloc(Vector3f.class));

      v.w.sub(tmp1, tmp2);
      v.w.scaleAdd(margin, d, v.w);
    }
Exemplo n.º 7
0
  @Override
  public void batchedUnitVectorGetSupportingVertexWithoutMargin(
      Vector3f[] vectors, Vector3f[] supportVerticesOut, int numVectors) {
    float newDot;

    // JAVA NOTE: rewritten as code used W coord for temporary usage in Vector3
    // TODO: optimize it
    float[] wcoords = new float[numVectors];

    // use 'w' component of supportVerticesOut?
    {
      for (int i = 0; i < numVectors; i++) {
        // supportVerticesOut[i][3] = btScalar(-1e30);
        wcoords[i] = -1e30f;
      }
    }
    Vector3f vtx = Stack.alloc(Vector3f.class);
    for (int i = 0; i < points.size(); i++) {
      VectorUtil.mul(vtx, points.getQuick(i), localScaling);

      for (int j = 0; j < numVectors; j++) {
        Vector3f vec = vectors[j];

        newDot = vec.dot(vtx);
        // if (newDot > supportVerticesOut[j][3])
        if (newDot > wcoords[j]) {
          // WARNING: don't swap next lines, the w component would get overwritten!
          supportVerticesOut[j].set(vtx);
          // supportVerticesOut[j][3] = newDot;
          wcoords[j] = newDot;
        }
      }
    }
  }
  protected void updateActivationState(float timeStep) {
    Vector3f tmp = Stack.alloc(Vector3f.class);

    for (int i = 0; i < collisionObjects.size(); i++) {
      CollisionObject colObj = collisionObjects.getQuick(i);
      RigidBody body = RigidBody.upcast(colObj);
      if (body != null) {
        body.updateDeactivation(timeStep);

        if (body.wantsSleeping()) {
          if (body.isStaticOrKinematicObject()) {
            body.setActivationState(CollisionObject.ISLAND_SLEEPING);
          } else {
            if (body.getActivationState() == CollisionObject.ACTIVE_TAG) {
              body.setActivationState(CollisionObject.WANTS_DEACTIVATION);
            }
            if (body.getActivationState() == CollisionObject.ISLAND_SLEEPING) {
              tmp.set(0f, 0f, 0f);
              body.setAngularVelocity(tmp);
              body.setLinearVelocity(tmp);
            }
          }
        } else {
          if (body.getActivationState() != CollisionObject.DISABLE_DEACTIVATION) {
            body.setActivationState(CollisionObject.ACTIVE_TAG);
          }
        }
      }
    }
  }
 public void awakenRigidBodiesInArea(Vector3f min, Vector3f max) {
   for (int i = 0; i < collisionObjects.size(); i++) {
     CollisionObject collisionObject = collisionObjects.getQuick(i);
     if (!collisionObject.isStaticOrKinematicObject() && !collisionObject.isActive()) {
       Vector3f otherMin = Stack.alloc(Vector3f.class);
       Vector3f otherMax = Stack.alloc(Vector3f.class);
       collisionObject
           .getCollisionShape()
           .getAabb(
               collisionObject.getWorldTransform(Stack.alloc(Transform.class)),
               otherMin,
               otherMax);
       if (AabbUtil2.testAabbAgainstAabb2(min, max, otherMin, otherMax)) {
         collisionObject.activate();
       }
     }
   }
 }
Exemplo n.º 10
0
    public Vector3f LocalSupport(Vector3f d, /*unsigned*/ int i, Vector3f out) {
      Vector3f tmp = Stack.alloc(Vector3f.class);
      MatrixUtil.transposeTransform(tmp, d, wrotations[i]);

      shapes[i].localGetSupportingVertex(tmp, out);
      wrotations[i].transform(out);
      out.add(positions[i]);

      return out;
    }
    @Override
    public float addSingleResult(LocalConvexResult convexResult, boolean normalInWorldSpace) {
      if (convexResult.hitCollisionObject == me) {
        return 1f;
      }

      Vector3f linVelA = Stack.alloc(Vector3f.class), linVelB = Stack.alloc(Vector3f.class);
      linVelA.sub(convexToWorld, convexFromWorld);
      linVelB.set(0f, 0f, 0f); // toB.getOrigin()-fromB.getOrigin();

      Vector3f relativeVelocity = Stack.alloc(Vector3f.class);
      relativeVelocity.sub(linVelA, linVelB);
      // don't report time of impact for motion away from the contact normal (or causes minor
      // penetration)
      if (convexResult.hitNormalLocal.dot(relativeVelocity) >= -allowedPenetration) {
        return 1f;
      }

      return super.addSingleResult(convexResult, normalInWorldSpace);
    }
  public void internalProcessAllTriangles(
      InternalTriangleIndexCallback callback, Vector3f aabbMin, Vector3f aabbMax) {
    int graphicssubparts = getNumSubParts();
    Vector3f[] triangle /*[3]*/ =
        new Vector3f[] {
          Stack.alloc(Vector3f.class), Stack.alloc(Vector3f.class), Stack.alloc(Vector3f.class)
        };

    Vector3f meshScaling = getScaling(Stack.alloc(Vector3f.class));

    for (int part = 0; part < graphicssubparts; part++) {
      VertexData data = getLockedReadOnlyVertexIndexBase(part);

      for (int i = 0, cnt = data.getIndexCount() / 3; i < cnt; i++) {
        data.getTriangle(i * 3, meshScaling, triangle);
        callback.internalProcessTriangleIndex(triangle, part, i);
      }

      unLockReadOnlyVertexBase(part);
    }
  }
  protected void synchronizeMotionStates() {
    Transform interpolatedTransform = Stack.alloc(Transform.class);

    Transform tmpTrans = Stack.alloc(Transform.class);
    Vector3f tmpLinVel = Stack.alloc(Vector3f.class);
    Vector3f tmpAngVel = Stack.alloc(Vector3f.class);

    // todo: iterate over awake simulation islands!
    for (int i = 0; i < collisionObjects.size(); i++) {
      CollisionObject colObj = collisionObjects.getQuick(i);

      RigidBody body = RigidBody.upcast(colObj);
      if (body != null && body.getMotionState() != null && !body.isStaticOrKinematicObject()) {
        // we need to call the update at least once, even for sleeping objects
        // otherwise the 'graphics' transform never updates properly
        // so todo: add 'dirty' flag
        // if (body->getActivationState() != ISLAND_SLEEPING)
        {
          TransformUtil.integrateTransform(
              body.getInterpolationWorldTransform(tmpTrans),
              body.getInterpolationLinearVelocity(tmpLinVel),
              body.getInterpolationAngularVelocity(tmpAngVel),
              localTime * body.getHitFraction(),
              interpolatedTransform);
          body.getMotionState().setWorldTransform(interpolatedTransform);
        }
      }
    }

    if (getDebugDrawer() != null
        && (getDebugDrawer().getDebugMode() & DebugDrawModes.DRAW_WIREFRAME) != 0) {
      for (int i = 0; i < vehicles.size(); i++) {
        for (int v = 0; v < vehicles.getQuick(i).getNumWheels(); v++) {
          // synchronize the wheels with the (interpolated) chassis worldtransform
          vehicles.getQuick(i).updateWheelTransform(v, true);
        }
      }
    }
  }
  protected void debugDrawSphere(float radius, Transform transform, Vector3f color) {
    Vector3f start = Stack.alloc(transform.origin);

    Vector3f xoffs = Stack.alloc(Vector3f.class);
    xoffs.set(radius, 0, 0);
    transform.basis.transform(xoffs);
    Vector3f yoffs = Stack.alloc(Vector3f.class);
    yoffs.set(0, radius, 0);
    transform.basis.transform(yoffs);
    Vector3f zoffs = Stack.alloc(Vector3f.class);
    zoffs.set(0, 0, radius);
    transform.basis.transform(zoffs);

    Vector3f tmp1 = Stack.alloc(Vector3f.class);
    Vector3f tmp2 = Stack.alloc(Vector3f.class);

    // XY
    tmp1.sub(start, xoffs);
    tmp2.add(start, yoffs);
    getDebugDrawer().drawLine(tmp1, tmp2, color);
    tmp1.add(start, yoffs);
    tmp2.add(start, xoffs);
    getDebugDrawer().drawLine(tmp1, tmp2, color);
    tmp1.add(start, xoffs);
    tmp2.sub(start, yoffs);
    getDebugDrawer().drawLine(tmp1, tmp2, color);
    tmp1.sub(start, yoffs);
    tmp2.sub(start, xoffs);
    getDebugDrawer().drawLine(tmp1, tmp2, color);

    // XZ
    tmp1.sub(start, xoffs);
    tmp2.add(start, zoffs);
    getDebugDrawer().drawLine(tmp1, tmp2, color);
    tmp1.add(start, zoffs);
    tmp2.add(start, xoffs);
    getDebugDrawer().drawLine(tmp1, tmp2, color);
    tmp1.add(start, xoffs);
    tmp2.sub(start, zoffs);
    getDebugDrawer().drawLine(tmp1, tmp2, color);
    tmp1.sub(start, zoffs);
    tmp2.sub(start, xoffs);
    getDebugDrawer().drawLine(tmp1, tmp2, color);

    // YZ
    tmp1.sub(start, yoffs);
    tmp2.add(start, zoffs);
    getDebugDrawer().drawLine(tmp1, tmp2, color);
    tmp1.add(start, zoffs);
    tmp2.add(start, yoffs);
    getDebugDrawer().drawLine(tmp1, tmp2, color);
    tmp1.add(start, yoffs);
    tmp2.sub(start, zoffs);
    getDebugDrawer().drawLine(tmp1, tmp2, color);
    tmp1.sub(start, zoffs);
    tmp2.sub(start, yoffs);
    getDebugDrawer().drawLine(tmp1, tmp2, color);
  }
Exemplo n.º 15
0
  @Override
  public Vector3f localGetSupportingVertex(Vector3f vec, Vector3f out) {
    Vector3f supVertex = localGetSupportingVertexWithoutMargin(vec, out);

    if (getMargin() != 0f) {
      Vector3f vecnorm = Stack.alloc(vec);
      if (vecnorm.lengthSquared() < (BulletGlobals.FLT_EPSILON * BulletGlobals.FLT_EPSILON)) {
        vecnorm.set(-1f, -1f, -1f);
      }
      vecnorm.normalize();
      supVertex.scaleAdd(getMargin(), vecnorm, supVertex);
    }
    return out;
  }
Exemplo n.º 16
0
    public boolean SolveSimplex3a(Vector3f ao, Vector3f ab, Vector3f ac, Vector3f cabc) {
      // TODO: optimize

      Vector3f tmp = Stack.alloc(Vector3f.class);
      tmp.cross(cabc, ab);

      Vector3f tmp2 = Stack.alloc(Vector3f.class);
      tmp2.cross(cabc, ac);

      if (tmp.dot(ao) < -GJK_insimplex_eps) {
        order = 1;
        simplex[0].set(simplex[1]);
        simplex[1].set(simplex[2]);
        return SolveSimplex2(ao, ab);
      } else if (tmp2.dot(ao) > +GJK_insimplex_eps) {
        order = 1;
        simplex[1].set(simplex[2]);
        return SolveSimplex2(ao, ac);
      } else {
        float d = cabc.dot(ao);
        if (Math.abs(d) > GJK_insimplex_eps) {
          if (d > 0) {
            ray.set(cabc);
          } else {
            ray.negate(cabc);

            Mkv swapTmp = new Mkv();
            swapTmp.set(simplex[0]);
            simplex[0].set(simplex[1]);
            simplex[1].set(swapTmp);
          }
          return false;
        } else {
          return true;
        }
      }
    }
Exemplo n.º 17
0
    public boolean SolveSimplex4(Vector3f ao, Vector3f ab, Vector3f ac, Vector3f ad) {
      // TODO: optimize

      Vector3f crs = Stack.alloc(Vector3f.class);

      Vector3f tmp = Stack.alloc(Vector3f.class);
      tmp.cross(ab, ac);

      Vector3f tmp2 = Stack.alloc(Vector3f.class);
      tmp2.cross(ac, ad);

      Vector3f tmp3 = Stack.alloc(Vector3f.class);
      tmp3.cross(ad, ab);

      if (tmp.dot(ao) > GJK_insimplex_eps) {
        crs.set(tmp);
        order = 2;
        simplex[0].set(simplex[1]);
        simplex[1].set(simplex[2]);
        simplex[2].set(simplex[3]);
        return SolveSimplex3a(ao, ab, ac, crs);
      } else if (tmp2.dot(ao) > GJK_insimplex_eps) {
        crs.set(tmp2);
        order = 2;
        simplex[2].set(simplex[3]);
        return SolveSimplex3a(ao, ac, ad, crs);
      } else if (tmp3.dot(ao) > GJK_insimplex_eps) {
        crs.set(tmp3);
        order = 2;
        simplex[1].set(simplex[0]);
        simplex[0].set(simplex[2]);
        simplex[2].set(simplex[3]);
        return SolveSimplex3a(ao, ad, ab, crs);
      } else {
        return (true);
      }
    }
Exemplo n.º 18
0
 public boolean SolveSimplex2(Vector3f ao, Vector3f ab) {
   if (ab.dot(ao) >= 0) {
     Vector3f cabo = Stack.alloc(Vector3f.class);
     cabo.cross(ab, ao);
     if (cabo.lengthSquared() > GJK_sqinsimplex_eps) {
       ray.cross(cabo, ab);
     } else {
       return true;
     }
   } else {
     order = 0;
     simplex[0].set(simplex[1]);
     ray.set(ao);
   }
   return (false);
 }
  protected void predictUnconstraintMotion(float timeStep) {
    Transform tmpTrans = Stack.alloc(Transform.class);

    for (int i = 0; i < collisionObjects.size(); i++) {
      CollisionObject colObj = collisionObjects.getQuick(i);
      RigidBody body = RigidBody.upcast(colObj);
      if (body != null) {
        if (!body.isStaticOrKinematicObject()) {
          if (body.isActive()) {
            body.integrateVelocities(timeStep);
            // damping
            body.applyDamping(timeStep);

            body.predictIntegratedTransform(
                timeStep, body.getInterpolationWorldTransform(tmpTrans));
          }
        }
      }
    }
  }
  public void debugDrawObject(Transform worldTransform, CollisionShape shape, Vector3f color) {
    Vector3f tmp = Stack.alloc(Vector3f.class);
    Vector3f tmp2 = Stack.alloc(Vector3f.class);

    // Draw a small simplex at the center of the object
    {
      Vector3f start = Stack.alloc(worldTransform.origin);

      tmp.set(1f, 0f, 0f);
      worldTransform.basis.transform(tmp);
      tmp.add(start);
      tmp2.set(1f, 0f, 0f);
      getDebugDrawer().drawLine(start, tmp, tmp2);

      tmp.set(0f, 1f, 0f);
      worldTransform.basis.transform(tmp);
      tmp.add(start);
      tmp2.set(0f, 1f, 0f);
      getDebugDrawer().drawLine(start, tmp, tmp2);

      tmp.set(0f, 0f, 1f);
      worldTransform.basis.transform(tmp);
      tmp.add(start);
      tmp2.set(0f, 0f, 1f);
      getDebugDrawer().drawLine(start, tmp, tmp2);
    }

    // JAVA TODO: debugDrawObject, note that this commented code is from old version, use actual
    // version when implementing

    //		if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
    //		{
    //			const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape);
    //			for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--)
    //			{
    //				btTransform childTrans = compoundShape->getChildTransform(i);
    //				const btCollisionShape* colShape = compoundShape->getChildShape(i);
    //				debugDrawObject(worldTransform*childTrans,colShape,color);
    //			}
    //
    //		} else
    //		{
    //			switch (shape->getShapeType())
    //			{
    //
    //			case SPHERE_SHAPE_PROXYTYPE:
    //				{
    //					const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape);
    //					btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw
    // with margin
    //
    //					debugDrawSphere(radius, worldTransform, color);
    //					break;
    //				}
    //			case MULTI_SPHERE_SHAPE_PROXYTYPE:
    //				{
    //					const btMultiSphereShape* multiSphereShape = static_cast<const
    // btMultiSphereShape*>(shape);
    //
    //					for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--)
    //					{
    //						btTransform childTransform = worldTransform;
    //						childTransform.getOrigin() += multiSphereShape->getSpherePosition(i);
    //						debugDrawSphere(multiSphereShape->getSphereRadius(i), childTransform, color);
    //					}
    //
    //					break;
    //				}
    //			case CAPSULE_SHAPE_PROXYTYPE:
    //				{
    //					const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape);
    //
    //					btScalar radius = capsuleShape->getRadius();
    //					btScalar halfHeight = capsuleShape->getHalfHeight();
    //
    //					// Draw the ends
    //					{
    //						btTransform childTransform = worldTransform;
    //						childTransform.getOrigin() = worldTransform * btVector3(0,halfHeight,0);
    //						debugDrawSphere(radius, childTransform, color);
    //					}
    //
    //					{
    //						btTransform childTransform = worldTransform;
    //						childTransform.getOrigin() = worldTransform * btVector3(0,-halfHeight,0);
    //						debugDrawSphere(radius, childTransform, color);
    //					}
    //
    //					// Draw some additional lines
    //					btVector3 start = worldTransform.getOrigin();
    //					getDebugDrawer()->drawLine(start+worldTransform.getBasis() *
    // btVector3(-radius,halfHeight,0),start+worldTransform.getBasis() *
    // btVector3(-radius,-halfHeight,0), color);
    //					getDebugDrawer()->drawLine(start+worldTransform.getBasis() *
    // btVector3(radius,halfHeight,0),start+worldTransform.getBasis() *
    // btVector3(radius,-halfHeight,0), color);
    //					getDebugDrawer()->drawLine(start+worldTransform.getBasis() *
    // btVector3(0,halfHeight,-radius),start+worldTransform.getBasis() *
    // btVector3(0,-halfHeight,-radius), color);
    //					getDebugDrawer()->drawLine(start+worldTransform.getBasis() *
    // btVector3(0,halfHeight,radius),start+worldTransform.getBasis() *
    // btVector3(0,-halfHeight,radius), color);
    //
    //					break;
    //				}
    //			case CONE_SHAPE_PROXYTYPE:
    //				{
    //					const btConeShape* coneShape = static_cast<const btConeShape*>(shape);
    //					btScalar radius = coneShape->getRadius();//+coneShape->getMargin();
    //					btScalar height = coneShape->getHeight();//+coneShape->getMargin();
    //					btVector3 start = worldTransform.getOrigin();
    //
    //					int upAxis= coneShape->getConeUpIndex();
    //
    //
    //					btVector3	offsetHeight(0,0,0);
    //					offsetHeight[upAxis] = height * btScalar(0.5);
    //					btVector3	offsetRadius(0,0,0);
    //					offsetRadius[(upAxis+1)%3] = radius;
    //					btVector3	offset2Radius(0,0,0);
    //					offset2Radius[(upAxis+2)%3] = radius;
    //
    //					getDebugDrawer()->drawLine(start+worldTransform.getBasis() *
    // (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
    //					getDebugDrawer()->drawLine(start+worldTransform.getBasis() *
    // (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
    //					getDebugDrawer()->drawLine(start+worldTransform.getBasis() *
    // (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offset2Radius),color);
    //					getDebugDrawer()->drawLine(start+worldTransform.getBasis() *
    // (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offset2Radius),color);
    //
    //
    //
    //					break;
    //
    //				}
    //			case CYLINDER_SHAPE_PROXYTYPE:
    //				{
    //					const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape);
    //					int upAxis = cylinder->getUpAxis();
    //					btScalar radius = cylinder->getRadius();
    //					btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis];
    //					btVector3 start = worldTransform.getOrigin();
    //					btVector3	offsetHeight(0,0,0);
    //					offsetHeight[upAxis] = halfHeight;
    //					btVector3	offsetRadius(0,0,0);
    //					offsetRadius[(upAxis+1)%3] = radius;
    //					getDebugDrawer()->drawLine(start+worldTransform.getBasis() *
    // (offsetHeight+offsetRadius),start+worldTransform.getBasis() *
    // (-offsetHeight+offsetRadius),color);
    //					getDebugDrawer()->drawLine(start+worldTransform.getBasis() *
    // (offsetHeight-offsetRadius),start+worldTransform.getBasis() *
    // (-offsetHeight-offsetRadius),color);
    //					break;
    //				}
    //			default:
    //				{
    //
    //					if (shape->isConcave())
    //					{
    //						btConcaveShape* concaveMesh = (btConcaveShape*) shape;
    //
    //						//todo pass camera, for some culling
    //						btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
    //						btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
    //
    //						DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
    //						concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax);
    //
    //					}
    //
    //					if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE)
    //					{
    //						btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape;
    //						//todo: pass camera for some culling
    //						btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
    //						btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
    //						//DebugDrawcallback drawCallback;
    //						DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
    //
    //	convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
    //					}
    //
    //
    //					/// for polyhedral shapes
    //					if (shape->isPolyhedral())
    //					{
    //						btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape;
    //
    //						int i;
    //						for (i=0;i<polyshape->getNumEdges();i++)
    //						{
    //							btPoint3 a,b;
    //							polyshape->getEdge(i,a,b);
    //							btVector3 wa = worldTransform * a;
    //							btVector3 wb = worldTransform * b;
    //							getDebugDrawer()->drawLine(wa,wb,color);
    //
    //						}
    //
    //
    //					}
    //				}
    //			}
    //		}
  }
Exemplo n.º 21
0
 public boolean SolveSimplex3(Vector3f ao, Vector3f ab, Vector3f ac) {
   Vector3f tmp = Stack.alloc(Vector3f.class);
   tmp.cross(ab, ac);
   return (SolveSimplex3a(ao, ab, ac, tmp));
 }
  @Override
  public void debugDrawWorld() {
    if (getDebugDrawer() != null
        && (getDebugDrawer().getDebugMode() & DebugDrawModes.DRAW_CONTACT_POINTS) != 0) {
      int numManifolds = getDispatcher().getNumManifolds();
      Vector3f color = Stack.alloc(Vector3f.class);
      color.set(0f, 0f, 0f);
      for (int i = 0; i < numManifolds; i++) {
        PersistentManifold contactManifold = getDispatcher().getManifoldByIndexInternal(i);
        // btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0());
        // btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1());

        int numContacts = contactManifold.getNumContacts();
        for (int j = 0; j < numContacts; j++) {
          ManifoldPoint cp = contactManifold.getContactPoint(j);
          getDebugDrawer()
              .drawContactPoint(
                  cp.positionWorldOnB,
                  cp.normalWorldOnB,
                  cp.getDistance(),
                  cp.getLifeTime(),
                  color);
        }
      }
    }

    if (getDebugDrawer() != null
        && (getDebugDrawer().getDebugMode()
                & (DebugDrawModes.DRAW_WIREFRAME | DebugDrawModes.DRAW_AABB))
            != 0) {
      int i;

      Transform tmpTrans = Stack.alloc(Transform.class);
      Vector3f minAabb = Stack.alloc(Vector3f.class);
      Vector3f maxAabb = Stack.alloc(Vector3f.class);
      Vector3f colorvec = Stack.alloc(Vector3f.class);

      // todo: iterate over awake simulation islands!
      for (i = 0; i < collisionObjects.size(); i++) {
        CollisionObject colObj = collisionObjects.getQuick(i);
        if (getDebugDrawer() != null
            && (getDebugDrawer().getDebugMode() & DebugDrawModes.DRAW_WIREFRAME) != 0) {
          Vector3f color = Stack.alloc(Vector3f.class);
          color.set(255f, 255f, 255f);
          switch (colObj.getActivationState()) {
            case CollisionObject.ACTIVE_TAG:
              color.set(255f, 255f, 255f);
              break;
            case CollisionObject.ISLAND_SLEEPING:
              color.set(0f, 255f, 0f);
              break;
            case CollisionObject.WANTS_DEACTIVATION:
              color.set(0f, 255f, 255f);
              break;
            case CollisionObject.DISABLE_DEACTIVATION:
              color.set(255f, 0f, 0f);
              break;
            case CollisionObject.DISABLE_SIMULATION:
              color.set(255f, 255f, 0f);
              break;
            default:
              {
                color.set(255f, 0f, 0f);
              }
          }

          debugDrawObject(colObj.getWorldTransform(tmpTrans), colObj.getCollisionShape(), color);
        }
        if (debugDrawer != null && (debugDrawer.getDebugMode() & DebugDrawModes.DRAW_AABB) != 0) {
          colorvec.set(1f, 0f, 0f);
          colObj.getCollisionShape().getAabb(colObj.getWorldTransform(tmpTrans), minAabb, maxAabb);
          debugDrawer.drawAabb(minAabb, maxAabb, colorvec);
        }
      }

      Vector3f wheelColor = Stack.alloc(Vector3f.class);
      Vector3f wheelPosWS = Stack.alloc(Vector3f.class);
      Vector3f axle = Stack.alloc(Vector3f.class);
      Vector3f tmp = Stack.alloc(Vector3f.class);

      for (i = 0; i < vehicles.size(); i++) {
        for (int v = 0; v < vehicles.getQuick(i).getNumWheels(); v++) {
          wheelColor.set(0, 255, 255);
          if (vehicles.getQuick(i).getWheelInfo(v).raycastInfo.isInContact) {
            wheelColor.set(0, 0, 255);
          } else {
            wheelColor.set(255, 0, 255);
          }

          wheelPosWS.set(vehicles.getQuick(i).getWheelInfo(v).worldTransform.origin);

          axle.set(
              vehicles
                  .getQuick(i)
                  .getWheelInfo(v)
                  .worldTransform
                  .basis
                  .getElement(0, vehicles.getQuick(i).getRightAxis()),
              vehicles
                  .getQuick(i)
                  .getWheelInfo(v)
                  .worldTransform
                  .basis
                  .getElement(1, vehicles.getQuick(i).getRightAxis()),
              vehicles
                  .getQuick(i)
                  .getWheelInfo(v)
                  .worldTransform
                  .basis
                  .getElement(2, vehicles.getQuick(i).getRightAxis()));

          // m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_wheelAxleWS
          // debug wheels (cylinders)
          tmp.add(wheelPosWS, axle);
          debugDrawer.drawLine(wheelPosWS, tmp, wheelColor);
          debugDrawer.drawLine(
              wheelPosWS,
              vehicles.getQuick(i).getWheelInfo(v).raycastInfo.contactPointWS,
              wheelColor);
        }
      }

      if (getDebugDrawer() != null && getDebugDrawer().getDebugMode() != 0) {
        for (i = 0; i < actions.size(); i++) {
          actions.getQuick(i).debugDraw(debugDrawer);
        }
      }
    }
  }
Exemplo n.º 23
0
 public boolean SearchOrigin() {
   Vector3f tmp = Stack.alloc(Vector3f.class);
   tmp.set(1f, 0f, 0f);
   return SearchOrigin(tmp);
 }
Exemplo n.º 24
0
    public boolean EncloseOrigin() {
      Vector3f tmp = Stack.alloc(Vector3f.class);
      Vector3f tmp1 = Stack.alloc(Vector3f.class);
      Vector3f tmp2 = Stack.alloc(Vector3f.class);

      switch (order) {
          // Point
        case 0:
          break;
          // Line
        case 1:
          {
            Vector3f ab = Stack.alloc(Vector3f.class);
            ab.sub(simplex[1].w, simplex[0].w);

            Vector3f[] b =
                new Vector3f[] {
                  Stack.alloc(Vector3f.class),
                  Stack.alloc(Vector3f.class),
                  Stack.alloc(Vector3f.class)
                };
            b[0].set(1f, 0f, 0f);
            b[1].set(0f, 1f, 0f);
            b[2].set(0f, 0f, 1f);

            b[0].cross(ab, b[0]);
            b[1].cross(ab, b[1]);
            b[2].cross(ab, b[2]);

            float m[] =
                new float[] {b[0].lengthSquared(), b[1].lengthSquared(), b[2].lengthSquared()};

            Quat4f tmpQuat = Stack.alloc(Quat4f.class);
            tmp.normalize(ab);
            QuaternionUtil.setRotation(tmpQuat, tmp, cst2Pi / 3f);

            Matrix3f r = Stack.alloc(Matrix3f.class);
            MatrixUtil.setRotation(r, tmpQuat);

            Vector3f w = Stack.alloc(Vector3f.class);
            w.set(b[m[0] > m[1] ? m[0] > m[2] ? 0 : 2 : m[1] > m[2] ? 1 : 2]);

            tmp.normalize(w);
            Support(tmp, simplex[4]);
            r.transform(w);
            tmp.normalize(w);
            Support(tmp, simplex[2]);
            r.transform(w);
            tmp.normalize(w);
            Support(tmp, simplex[3]);
            r.transform(w);
            order = 4;
            return (true);
          }
          // Triangle
        case 2:
          {
            tmp1.sub(simplex[1].w, simplex[0].w);
            tmp2.sub(simplex[2].w, simplex[0].w);
            Vector3f n = Stack.alloc(Vector3f.class);
            n.cross(tmp1, tmp2);
            n.normalize();

            Support(n, simplex[3]);

            tmp.negate(n);
            Support(tmp, simplex[4]);
            order = 4;
            return (true);
          }
          // Tetrahedron
        case 3:
          return (true);
          // Hexahedron
        case 4:
          return (true);
      }
      return (false);
    }
Exemplo n.º 25
0
    public float EvaluatePD(float accuracy) {
      pushStack();
      try {
        Vector3f tmp = Stack.alloc(Vector3f.class);

        // btBlock* sablock = sa->beginBlock();
        Face bestface = null;
        int markid = 1;
        depth = -cstInf;
        normal.set(0f, 0f, 0f);
        root = null;
        nfaces = 0;
        iterations = 0;
        failed = false;
        /* Prepare hull		*/
        if (gjk.EncloseOrigin()) {
          // const U* pfidx = 0;
          int[][] pfidx_ptr = null;
          int pfidx_index = 0;

          int nfidx = 0;
          // const U* peidx = 0;
          int[][] peidx_ptr = null;
          int peidx_index = 0;

          int neidx = 0;
          Mkv[] basemkv = new Mkv[5];
          Face[] basefaces = new Face[6];
          switch (gjk.order) {
              // Tetrahedron
            case 3:
              {
                // pfidx=(const U*)fidx;
                pfidx_ptr = tetrahedron_fidx;
                pfidx_index = 0;

                nfidx = 4;

                // peidx=(const U*)eidx;
                peidx_ptr = tetrahedron_eidx;
                peidx_index = 0;

                neidx = 6;
              }
              break;
              // Hexahedron
            case 4:
              {
                // pfidx=(const U*)fidx;
                pfidx_ptr = hexahedron_fidx;
                pfidx_index = 0;

                nfidx = 6;

                // peidx=(const U*)eidx;
                peidx_ptr = hexahedron_eidx;
                peidx_index = 0;

                neidx = 9;
              }
              break;
          }
          int i;

          for (i = 0; i <= gjk.order; ++i) {
            basemkv[i] = new Mkv();
            basemkv[i].set(gjk.simplex[i]);
          }
          for (i = 0; i < nfidx; ++i, pfidx_index++) {
            basefaces[i] =
                NewFace(
                    basemkv[pfidx_ptr[pfidx_index][0]],
                    basemkv[pfidx_ptr[pfidx_index][1]],
                    basemkv[pfidx_ptr[pfidx_index][2]]);
          }
          for (i = 0; i < neidx; ++i, peidx_index++) {
            Link(
                basefaces[peidx_ptr[peidx_index][0]],
                peidx_ptr[peidx_index][1],
                basefaces[peidx_ptr[peidx_index][2]],
                peidx_ptr[peidx_index][3]);
          }
        }
        if (0 == nfaces) {
          // sa->endBlock(sablock);
          return (depth);
        }
        /* Expand hull		*/
        for (; iterations < EPA_maxiterations; ++iterations) {
          Face bf = FindBest();
          if (bf != null) {
            tmp.negate(bf.n);
            Mkv w = Support(tmp);
            float d = bf.n.dot(w.w) + bf.d;
            bestface = bf;
            if (d < -accuracy) {
              Face[] cf = new Face[] {null};
              Face[] ff = new Face[] {null};
              int nf = 0;
              Detach(bf);
              bf.mark = ++markid;
              for (int i = 0; i < 3; ++i) {
                nf += BuildHorizon(markid, w, bf.f[i], bf.e[i], cf, ff);
              }
              if (nf <= 2) {
                break;
              }
              Link(cf[0], 1, ff[0], 2);
            } else {
              break;
            }
          } else {
            break;
          }
        }
        /* Extract contact	*/
        if (bestface != null) {
          Vector3f b = GetCoordinates(bestface, Stack.alloc(Vector3f.class));
          normal.set(bestface.n);
          depth = Math.max(0, bestface.d);
          for (int i = 0; i < 2; ++i) {
            float s = i != 0 ? -1f : 1f;
            for (int j = 0; j < 3; ++j) {
              tmp.scale(s, bestface.v[j].r);
              gjk.LocalSupport(tmp, i, features[i][j]);
            }
          }

          Vector3f tmp1 = Stack.alloc(Vector3f.class);
          Vector3f tmp2 = Stack.alloc(Vector3f.class);
          Vector3f tmp3 = Stack.alloc(Vector3f.class);

          tmp1.scale(b.x, features[0][0]);
          tmp2.scale(b.y, features[0][1]);
          tmp3.scale(b.z, features[0][2]);
          VectorUtil.add(nearest[0], tmp1, tmp2, tmp3);

          tmp1.scale(b.x, features[1][0]);
          tmp2.scale(b.y, features[1][1]);
          tmp3.scale(b.z, features[1][2]);
          VectorUtil.add(nearest[1], tmp1, tmp2, tmp3);
        } else {
          failed = true;
        }
        // sa->endBlock(sablock);
        return (depth);
      } finally {
        popStack();
      }
    }
Exemplo n.º 26
0
  @Override
  public void solveConstraint(float timeStep) {
    Vector3f tmp = Stack.alloc(Vector3f.class);
    Vector3f tmp2 = Stack.alloc(Vector3f.class);

    Vector3f tmpVec = Stack.alloc(Vector3f.class);
    Transform tmpTrans = Stack.alloc(Transform.class);

    Vector3f pivotAInW = Stack.alloc(rbAFrame.origin);
    rbA.getCenterOfMassTransform(tmpTrans).transform(pivotAInW);

    Vector3f pivotBInW = Stack.alloc(rbBFrame.origin);
    rbB.getCenterOfMassTransform(tmpTrans).transform(pivotBInW);

    float tau = 0.3f;

    // linear part
    if (!angularOnly) {
      Vector3f rel_pos1 = Stack.alloc(Vector3f.class);
      rel_pos1.sub(pivotAInW, rbA.getCenterOfMassPosition(tmpVec));

      Vector3f rel_pos2 = Stack.alloc(Vector3f.class);
      rel_pos2.sub(pivotBInW, rbB.getCenterOfMassPosition(tmpVec));

      Vector3f vel1 = rbA.getVelocityInLocalPoint(rel_pos1, Stack.alloc(Vector3f.class));
      Vector3f vel2 = rbB.getVelocityInLocalPoint(rel_pos2, Stack.alloc(Vector3f.class));
      Vector3f vel = Stack.alloc(Vector3f.class);
      vel.sub(vel1, vel2);

      for (int i = 0; i < 3; i++) {
        Vector3f normal = jac[i].linearJointAxis;
        float jacDiagABInv = 1f / jac[i].getDiagonal();

        float rel_vel;
        rel_vel = normal.dot(vel);
        // positional error (zeroth order error)
        tmp.sub(pivotAInW, pivotBInW);
        float depth = -(tmp).dot(normal); // this is the error projected on the normal
        float impulse = depth * tau / timeStep * jacDiagABInv - rel_vel * jacDiagABInv;
        appliedImpulse += impulse;
        Vector3f impulse_vector = Stack.alloc(Vector3f.class);
        impulse_vector.scale(impulse, normal);

        tmp.sub(pivotAInW, rbA.getCenterOfMassPosition(tmpVec));
        rbA.applyImpulse(impulse_vector, tmp);

        tmp.negate(impulse_vector);
        tmp2.sub(pivotBInW, rbB.getCenterOfMassPosition(tmpVec));
        rbB.applyImpulse(tmp, tmp2);
      }
    }

    {
      // solve angular part
      Vector3f angVelA = getRigidBodyA().getAngularVelocity(Stack.alloc(Vector3f.class));
      Vector3f angVelB = getRigidBodyB().getAngularVelocity(Stack.alloc(Vector3f.class));

      // solve swing limit
      if (solveSwingLimit) {
        tmp.sub(angVelB, angVelA);
        float amplitude =
            ((tmp).dot(swingAxis) * relaxationFactor * relaxationFactor
                + swingCorrection * (1f / timeStep) * biasFactor);
        float impulseMag = amplitude * kSwing;

        // Clamp the accumulated impulse
        float temp = accSwingLimitImpulse;
        accSwingLimitImpulse = Math.max(accSwingLimitImpulse + impulseMag, 0.0f);
        impulseMag = accSwingLimitImpulse - temp;

        Vector3f impulse = Stack.alloc(Vector3f.class);
        impulse.scale(impulseMag, swingAxis);

        rbA.applyTorqueImpulse(impulse);

        tmp.negate(impulse);
        rbB.applyTorqueImpulse(tmp);
      }

      // solve twist limit
      if (solveTwistLimit) {
        tmp.sub(angVelB, angVelA);
        float amplitude =
            ((tmp).dot(twistAxis) * relaxationFactor * relaxationFactor
                + twistCorrection * (1f / timeStep) * biasFactor);
        float impulseMag = amplitude * kTwist;

        // Clamp the accumulated impulse
        float temp = accTwistLimitImpulse;
        accTwistLimitImpulse = Math.max(accTwistLimitImpulse + impulseMag, 0.0f);
        impulseMag = accTwistLimitImpulse - temp;

        Vector3f impulse = Stack.alloc(Vector3f.class);
        impulse.scale(impulseMag, twistAxis);

        rbA.applyTorqueImpulse(impulse);

        tmp.negate(impulse);
        rbB.applyTorqueImpulse(tmp);
      }
    }
  }
Exemplo n.º 27
0
  @Override
  public void buildJacobian() {
    Vector3f tmp = Stack.alloc(Vector3f.class);
    Vector3f tmp1 = Stack.alloc(Vector3f.class);
    Vector3f tmp2 = Stack.alloc(Vector3f.class);

    Transform tmpTrans = Stack.alloc(Transform.class);

    appliedImpulse = 0f;

    // set bias, sign, clear accumulator
    swingCorrection = 0f;
    twistLimitSign = 0f;
    solveTwistLimit = false;
    solveSwingLimit = false;
    accTwistLimitImpulse = 0f;
    accSwingLimitImpulse = 0f;

    if (!angularOnly) {
      Vector3f pivotAInW = Stack.alloc(rbAFrame.origin);
      rbA.getCenterOfMassTransform(tmpTrans).transform(pivotAInW);

      Vector3f pivotBInW = Stack.alloc(rbBFrame.origin);
      rbB.getCenterOfMassTransform(tmpTrans).transform(pivotBInW);

      Vector3f relPos = Stack.alloc(Vector3f.class);
      relPos.sub(pivotBInW, pivotAInW);

      // TODO: stack
      Vector3f[] normal /*[3]*/ =
          new Vector3f[] {
            Stack.alloc(Vector3f.class), Stack.alloc(Vector3f.class), Stack.alloc(Vector3f.class)
          };
      if (relPos.lengthSquared() > BulletGlobals.FLT_EPSILON) {
        normal[0].normalize(relPos);
      } else {
        normal[0].set(1f, 0f, 0f);
      }

      TransformUtil.planeSpace1(normal[0], normal[1], normal[2]);

      for (int i = 0; i < 3; i++) {
        Matrix3f mat1 = rbA.getCenterOfMassTransform(Stack.alloc(Transform.class)).basis;
        mat1.transpose();

        Matrix3f mat2 = rbB.getCenterOfMassTransform(Stack.alloc(Transform.class)).basis;
        mat2.transpose();

        tmp1.sub(pivotAInW, rbA.getCenterOfMassPosition(tmp));
        tmp2.sub(pivotBInW, rbB.getCenterOfMassPosition(tmp));

        jac[i].init(
            mat1,
            mat2,
            tmp1,
            tmp2,
            normal[i],
            rbA.getInvInertiaDiagLocal(Stack.alloc(Vector3f.class)),
            rbA.getInvMass(),
            rbB.getInvInertiaDiagLocal(Stack.alloc(Vector3f.class)),
            rbB.getInvMass());
      }
    }

    Vector3f b1Axis1 = Stack.alloc(Vector3f.class),
        b1Axis2 = Stack.alloc(Vector3f.class),
        b1Axis3 = Stack.alloc(Vector3f.class);
    Vector3f b2Axis1 = Stack.alloc(Vector3f.class), b2Axis2 = Stack.alloc(Vector3f.class);

    rbAFrame.basis.getColumn(0, b1Axis1);
    getRigidBodyA().getCenterOfMassTransform(tmpTrans).basis.transform(b1Axis1);

    rbBFrame.basis.getColumn(0, b2Axis1);
    getRigidBodyB().getCenterOfMassTransform(tmpTrans).basis.transform(b2Axis1);

    float swing1 = 0f, swing2 = 0f;

    float swx = 0f, swy = 0f;
    float thresh = 10f;
    float fact;

    // Get Frame into world space
    if (swingSpan1 >= 0.05f) {
      rbAFrame.basis.getColumn(1, b1Axis2);
      getRigidBodyA().getCenterOfMassTransform(tmpTrans).basis.transform(b1Axis2);
      //			swing1 = ScalarUtil.atan2Fast(b2Axis1.dot(b1Axis2), b2Axis1.dot(b1Axis1));
      swx = b2Axis1.dot(b1Axis1);
      swy = b2Axis1.dot(b1Axis2);
      swing1 = ScalarUtil.atan2Fast(swy, swx);
      fact = (swy * swy + swx * swx) * thresh * thresh;
      fact = fact / (fact + 1f);
      swing1 *= fact;
    }

    if (swingSpan2 >= 0.05f) {
      rbAFrame.basis.getColumn(2, b1Axis3);
      getRigidBodyA().getCenterOfMassTransform(tmpTrans).basis.transform(b1Axis3);
      //			swing2 = ScalarUtil.atan2Fast(b2Axis1.dot(b1Axis3), b2Axis1.dot(b1Axis1));
      swx = b2Axis1.dot(b1Axis1);
      swy = b2Axis1.dot(b1Axis3);
      swing2 = ScalarUtil.atan2Fast(swy, swx);
      fact = (swy * swy + swx * swx) * thresh * thresh;
      fact = fact / (fact + 1f);
      swing2 *= fact;
    }

    float RMaxAngle1Sq = 1.0f / (swingSpan1 * swingSpan1);
    float RMaxAngle2Sq = 1.0f / (swingSpan2 * swingSpan2);
    float EllipseAngle =
        Math.abs(swing1 * swing1) * RMaxAngle1Sq + Math.abs(swing2 * swing2) * RMaxAngle2Sq;

    if (EllipseAngle > 1.0f) {
      swingCorrection = EllipseAngle - 1.0f;
      solveSwingLimit = true;

      // Calculate necessary axis & factors
      tmp1.scale(b2Axis1.dot(b1Axis2), b1Axis2);
      tmp2.scale(b2Axis1.dot(b1Axis3), b1Axis3);
      tmp.add(tmp1, tmp2);
      swingAxis.cross(b2Axis1, tmp);
      swingAxis.normalize();

      float swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
      swingAxis.scale(swingAxisSign);

      kSwing =
          1f
              / (getRigidBodyA().computeAngularImpulseDenominator(swingAxis)
                  + getRigidBodyB().computeAngularImpulseDenominator(swingAxis));
    }

    // Twist limits
    if (twistSpan >= 0f) {
      // Vector3f b2Axis2 = Stack.alloc(Vector3f.class);
      rbBFrame.basis.getColumn(1, b2Axis2);
      getRigidBodyB().getCenterOfMassTransform(tmpTrans).basis.transform(b2Axis2);

      Quat4f rotationArc =
          QuaternionUtil.shortestArcQuat(b2Axis1, b1Axis1, Stack.alloc(Quat4f.class));
      Vector3f TwistRef =
          QuaternionUtil.quatRotate(rotationArc, b2Axis2, Stack.alloc(Vector3f.class));
      float twist = ScalarUtil.atan2Fast(TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2));

      float lockedFreeFactor = (twistSpan > 0.05f) ? limitSoftness : 0f;
      if (twist <= -twistSpan * lockedFreeFactor) {
        twistCorrection = -(twist + twistSpan);
        solveTwistLimit = true;

        twistAxis.add(b2Axis1, b1Axis1);
        twistAxis.scale(0.5f);
        twistAxis.normalize();
        twistAxis.scale(-1.0f);

        kTwist =
            1f
                / (getRigidBodyA().computeAngularImpulseDenominator(twistAxis)
                    + getRigidBodyB().computeAngularImpulseDenominator(twistAxis));

      } else if (twist > twistSpan * lockedFreeFactor) {
        twistCorrection = (twist - twistSpan);
        solveTwistLimit = true;

        twistAxis.add(b2Axis1, b1Axis1);
        twistAxis.scale(0.5f);
        twistAxis.normalize();

        kTwist =
            1f
                / (getRigidBodyA().computeAngularImpulseDenominator(twistAxis)
                    + getRigidBodyB().computeAngularImpulseDenominator(twistAxis));
      }
    }
  }