Java 类com.badlogic.gdx.math.Matrix3 实例源码

项目:libgdx-jbullet    文件:Transform.java   
public Matrix4 getMatrix(Matrix4 out) {
    out.val[Matrix4.M00] = basis.val[Matrix3.M00];
    out.val[Matrix4.M01] = basis.val[Matrix3.M01];
    out.val[Matrix4.M02] = basis.val[Matrix3.M02];
    out.val[Matrix4.M03] = origin.x;
    out.val[Matrix4.M10] = basis.val[Matrix3.M10];
    out.val[Matrix4.M11] = basis.val[Matrix3.M11];
    out.val[Matrix4.M12] = basis.val[Matrix3.M12];
    out.val[Matrix4.M13] = origin.y;
    out.val[Matrix4.M20] = basis.val[Matrix3.M20];
    out.val[Matrix4.M21] = basis.val[Matrix3.M21];
    out.val[Matrix4.M22] = basis.val[Matrix3.M22];
    out.val[Matrix4.M23] = origin.z;
    out.val[Matrix4.M30] = 0;
    out.val[Matrix4.M31] = 0;
    out.val[Matrix4.M32] = 0;
    out.val[Matrix4.M33] = 1;
    return out;
}
项目:libgdx-jbullet    文件:MatrixUtil.java   
public static void getColumn (Matrix3 src, int col, Vector3 v) {
    if (col == 0) {
        v.x = src.val[Matrix3.M00];
        v.y = src.val[Matrix3.M10];
        v.z = src.val[Matrix3.M20];
    } else if (col == 1) {
        v.x = src.val[Matrix3.M01];
        v.y = src.val[Matrix3.M11];
        v.z = src.val[Matrix3.M21];
    } else if (col == 2) {
        v.x = src.val[Matrix3.M02];
        v.y = src.val[Matrix3.M12];
        v.z = src.val[Matrix3.M22];
    } else {
        throw new GdxRuntimeException("Invalid column");
    }
}
项目:libgdx-jbullet    文件:MatrixUtil.java   
public static void setColumn (Matrix3 src, int col, Vector3 v) {
    if (col == 0) {
        src.val[Matrix3.M00] = v.x;
        src.val[Matrix3.M10] = v.y;
        src.val[Matrix3.M20] = v.z;
    } else if (col == 1) {
        src.val[Matrix3.M01] = v.x;
        src.val[Matrix3.M11] = v.y;
        src.val[Matrix3.M21] = v.z;
    } else if (col == 2) {
        src.val[Matrix3.M02] = v.x;
        src.val[Matrix3.M12] = v.y;
        src.val[Matrix3.M22] = v.z;
    } else {
        throw new GdxRuntimeException("Invalid column");
    }
}
项目:libgdx-jbullet    文件:MatrixUtil.java   
public static void getRow (Matrix3 src, int row, Vector3 v) {
    if (row == 0) {
        v.x = src.val[Matrix3.M00];
        v.y = src.val[Matrix3.M01];
        v.z = src.val[Matrix3.M02];
    } else if (row == 1) {
        v.x = src.val[Matrix3.M10];
        v.y = src.val[Matrix3.M11];
        v.z = src.val[Matrix3.M12];
    } else if (row == 2) {
        v.x = src.val[Matrix3.M20];
        v.y = src.val[Matrix3.M21];
        v.z = src.val[Matrix3.M22];
    } else {
        throw new GdxRuntimeException("Invalid row");
    }
}
项目:libgdx-jbullet    文件:MatrixUtil.java   
public static void setRow (Matrix3 src, int row, float x, float y, float z) {
    if (row == 0) {
        src.val[Matrix3.M00] = x;
        src.val[Matrix3.M01] = y;
        src.val[Matrix3.M02] = z;
    } else if (row == 1) {
        src.val[Matrix3.M10] = x;
        src.val[Matrix3.M11] = y;
        src.val[Matrix3.M12] = z;
    } else if (row == 2) {
        src.val[Matrix3.M20] = x;
        src.val[Matrix3.M21] = y;
        src.val[Matrix3.M22] = z;
    } else {
        throw new GdxRuntimeException("Invalid row");
    }
}
项目:libgdx-jbullet    文件:MatrixUtil.java   
/** Sets rotation matrix from euler angles. The euler angles are applied in ZYX order. This means a vector is first rotated
 * about X then Y and then Z axis. */
public static void setEulerZYX (Matrix3 mat, float eulerX, float eulerY, float eulerZ) {
    float ci = (float)Math.cos(eulerX);
    float cj = (float)Math.cos(eulerY);
    float ch = (float)Math.cos(eulerZ);
    float si = (float)Math.sin(eulerX);
    float sj = (float)Math.sin(eulerY);
    float sh = (float)Math.sin(eulerZ);
    float cc = ci * ch;
    float cs = ci * sh;
    float sc = si * ch;
    float ss = si * sh;

    setRow(mat, 0, cj * ch, sj * sc - cs, sj * cc + ss);
    setRow(mat, 1, cj * sh, sj * ss + cc, sj * cs - sc);
    setRow(mat, 2, -sj, cj * si, cj * ci);
}
项目:libgdx-jbullet    文件:MatrixUtil.java   
public static void setRotation (Matrix3 dest, Quaternion q) {
    float d = q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w;
    assert (d != 0f);
    float s = 2f / d;
    float xs = q.x * s, ys = q.y * s, zs = q.z * s;
    float wx = q.w * xs, wy = q.w * ys, wz = q.w * zs;
    float xx = q.x * xs, xy = q.x * ys, xz = q.x * zs;
    float yy = q.y * ys, yz = q.y * zs, zz = q.z * zs;
    dest.val[Matrix3.M00] = 1f - (yy + zz);
    dest.val[Matrix3.M01] = xy - wz;
    dest.val[Matrix3.M02] = xz + wy;
    dest.val[Matrix3.M10] = xy + wz;
    dest.val[Matrix3.M11] = 1f - (xx + zz);
    dest.val[Matrix3.M12] = yz - wx;
    dest.val[Matrix3.M20] = xz - wy;
    dest.val[Matrix3.M21] = yz + wx;
    dest.val[Matrix3.M22] = 1f - (xx + yy);
}
项目:libgdx-jbullet    文件:MatrixUtil.java   
public static void getRotation (Matrix3 mat, Quaternion dest) {
    /*
     * ArrayPool<float[]> floatArrays = ArrayPool.get(float.class);
     * 
     * float trace = mat.val[Matrix3.M00] + mat.val[Matrix3.M11] + mat.val[Matrix3.M22]; float[] temp = floatArrays.getFixed(4);
     * 
     * if (trace > 0f) { float s = (float) Math.sqrt(trace + 1f); temp[3] = (s * 0.5f); s = 0.5f / s;
     * 
     * temp[0] = ((mat.val[Matrix3.M21] - mat.val[Matrix3.M12]) * s); temp[1] = ((mat.val[Matrix3.M02] - mat.val[Matrix3.M20]) *
     * s); temp[2] = ((mat.val[Matrix3.M10] - mat.val[Matrix3.M01]) * s); } else { int i = mat.val[Matrix3.M00] <
     * mat.val[Matrix3.M11] ? (mat.val[Matrix3.M11] < mat.val[Matrix3.M22] ? 2 : 1) : (mat.val[Matrix3.M00] <
     * mat.val[Matrix3.M22] ? 2 : 0); int j = (i + 1) % 3; int k = (i + 2) % 3;
     * 
     * float s = (float) Math.sqrt(mat.getElement(i, i) - mat.getElement(j, j) - mat.getElement(k, k) + 1f); temp[i] = s * 0.5f;
     * s = 0.5f / s;
     * 
     * temp[3] = (mat.getElement(k, j) - mat.getElement(j, k)) * s; temp[j] = (mat.getElement(j, i) + mat.getElement(i, j)) * s;
     * temp[k] = (mat.getElement(k, i) + mat.getElement(i, k)) * s; } dest.set(temp[0], temp[1], temp[2], temp[3]);
     * 
     * floatArrays.release(temp);
     */
    // FIXME check this is correct
    dest.setFromMatrix(true, mat);
}
项目:libgdx-jbullet    文件:JacobianEntry.java   
/** Constraint between two different rigidbodies. */
public void init (Matrix3 world2A, Matrix3 world2B, Vector3 rel_pos1, Vector3 rel_pos2, Vector3 jointAxis, Vector3 inertiaInvA,
    float massInvA, Vector3 inertiaInvB, float massInvB) {
    linearJointAxis.set(jointAxis);

    aJ.set(rel_pos1).crs(linearJointAxis);
    aJ.mul(world2A);

    bJ.set(linearJointAxis);
    bJ.scl(-1);
    bJ.set(rel_pos2).crs(bJ);
    bJ.mul(world2B);

    VectorUtil.mul(m_0MinvJt, inertiaInvA, aJ);
    VectorUtil.mul(m_1MinvJt, inertiaInvB, bJ);
    Adiag = massInvA + m_0MinvJt.dot(aJ) + massInvB + m_1MinvJt.dot(bJ);

    assert (Adiag > 0f);
}
项目:libgdx-jbullet    文件:JacobianEntry.java   
/** Angular constraint between two different rigidbodies. */
public void init (Vector3 jointAxis, Matrix3 world2A, Matrix3 world2B, Vector3 inertiaInvA, Vector3 inertiaInvB) {
    linearJointAxis.set(0f, 0f, 0f);

    aJ.set(jointAxis);
    aJ.mul(world2A);

    bJ.set(jointAxis);
    bJ.scl(-1);
    bJ.mul(world2B);

    VectorUtil.mul(m_0MinvJt, inertiaInvA, aJ);
    VectorUtil.mul(m_1MinvJt, inertiaInvB, bJ);
    Adiag = m_0MinvJt.dot(aJ) + m_1MinvJt.dot(bJ);

    assert (Adiag > 0f);
}
项目:libgdx-jbullet    文件:JacobianEntry.java   
/** Constraint on one rigidbody. */
public void init (Matrix3 world2A, Vector3 rel_pos1, Vector3 rel_pos2, Vector3 jointAxis, Vector3 inertiaInvA,
    float massInvA) {
    linearJointAxis.set(jointAxis);

    aJ.set(rel_pos1).crs(jointAxis);
    aJ.mul(world2A);

    bJ.set(jointAxis);
    bJ.scl(-1);
    bJ.set(rel_pos2).crs(bJ);
    bJ.mul(world2A);

    VectorUtil.mul(m_0MinvJt, inertiaInvA, aJ);
    m_1MinvJt.set(0f, 0f, 0f);
    Adiag = massInvA + m_0MinvJt.dot(aJ);

    assert (Adiag > 0f);
}
项目:libgdx-jbullet    文件:HingeConstraint.java   
public HingeConstraint (RigidBody rbA, RigidBody rbB, Transform rbAFrame, Transform rbBFrame) {
    super(TypedConstraintType.HINGE_CONSTRAINT_TYPE, rbA, rbB);
    this.rbAFrame.set(rbAFrame);
    this.rbBFrame.set(rbBFrame);
    angularOnly = false;
    enableAngularMotor = false;

    // flip axis
    this.rbBFrame.basis.val[Matrix3.M02] *= -1f;
    this.rbBFrame.basis.val[Matrix3.M12] *= -1f;
    this.rbBFrame.basis.val[Matrix3.M22] *= -1f;

    // start with free
    lowerLimit = 1e30f;
    upperLimit = -1e30f;
    biasFactor = 0.3f;
    relaxationFactor = 1.0f;
    limitSoftness = 0.9f;
    solveLimit = false;
}
项目:libgdx-jbullet    文件:Generic6DofConstraint.java   
protected void buildLinearJacobian (/* JacobianEntry jacLinear */int jacLinear_index, Vector3 normalWorld, Vector3 pivotAInW,
    Vector3 pivotBInW) {
    Stack stack = Stack.enter();
    Matrix3 mat1 = rbA.getCenterOfMassTransform(stack.allocTransform()).basis;
    mat1.transpose();

    Matrix3 mat2 = rbB.getCenterOfMassTransform(stack.allocTransform()).basis;
    mat2.transpose();

    Vector3 tmpVec = stack.allocVector3();

    Vector3 tmp1 = stack.allocVector3();
    tmp1.set(pivotAInW).sub(rbA.getCenterOfMassPosition(tmpVec));

    Vector3 tmp2 = stack.allocVector3();
    tmp2.set(pivotBInW).sub(rbB.getCenterOfMassPosition(tmpVec));

    jacLinear[jacLinear_index].init(mat1, mat2, tmp1, tmp2, normalWorld, rbA.getInvInertiaDiagLocal(stack.allocVector3()),
        rbA.getInvMass(), rbB.getInvInertiaDiagLocal(stack.allocVector3()), rbB.getInvMass());
    stack.leave();
}
项目:ForgE    文件:UniformDefinition.java   
private String getTypeName() {
  if (customType != null) {
    return customType;
  } else if (type == Matrix4.class) {
    return "mat4";
  } else if (type == Float.class) {
    return "float";
  } else if (type == Matrix3.class) {
    return "mat3";
  } else if (type == Vector2.class) {
    return "vec2";
  } else if (type == Vector3.class || type == Color.class) {
    return "vec4";
  } else if (type == Texture.class) {
    return "sampler2D";
  }

  throw new GdxRuntimeException("Undefined type: " + type + " for " + name);
}
项目:gaiasky    文件:Matrix4d.java   
/**
 * Sets this matrix to the given 3x3 matrix. The third column of this matrix
 * is set to (0,0,1,0).
 * 
 * @param mat
 *            the matrix
 */
public Matrix4d set(Matrix3 mat) {
    val[0] = mat.val[0];
    val[1] = mat.val[1];
    val[2] = mat.val[2];
    val[3] = 0;
    val[4] = mat.val[3];
    val[5] = mat.val[4];
    val[6] = mat.val[5];
    val[7] = 0;
    val[8] = 0;
    val[9] = 0;
    val[10] = 1;
    val[11] = 0;
    val[12] = mat.val[6];
    val[13] = mat.val[7];
    val[14] = 0;
    val[15] = mat.val[8];
    return this;
}
项目:jenjin    文件:Bone.java   
public Matrix3 getWorldTransform (Matrix3 worldTransform) {
    if (worldTransform == null) throw new IllegalArgumentException("worldTransform cannot be null.");
    float[] val = worldTransform.val;
    val[M00] = a;
    val[M01] = b;
    val[M10] = c;
    val[M11] = d;
    val[M02] = worldX;
    val[M12] = worldY;
    val[M20] = 0;
    val[M21] = 0;
    val[M22] = 1;
    return worldTransform;
}
项目:libgdx-jbullet    文件:TriangleMeshShape.java   
@Override
public void getAabb (Transform trans, Vector3 aabbMin, Vector3 aabbMax) {
    Stack stack = Stack.enter();
    Vector3 tmp = stack.allocVector3();

    Vector3 localHalfExtents = stack.allocVector3();
    localHalfExtents.set(localAabbMax).sub(localAabbMin);
    localHalfExtents.scl(0.5f);

    Vector3 localCenter = stack.allocVector3();
    localCenter.set(localAabbMax).add(localAabbMin);
    localCenter.scl(0.5f);

    Matrix3 abs_b = stack.alloc(trans.basis);
    MatrixUtil.absolute(abs_b);

    Vector3 center = stack.alloc(localCenter);
    trans.transform(center);

    Vector3 extent = stack.allocVector3();
    MatrixUtil.getRow(abs_b, 0, tmp);
    extent.x = tmp.dot(localHalfExtents);
    MatrixUtil.getRow(abs_b, 1, tmp);
    extent.y = tmp.dot(localHalfExtents);
    MatrixUtil.getRow(abs_b, 2, tmp);
    extent.z = tmp.dot(localHalfExtents);

    Vector3 margin = stack.allocVector3();
    margin.set(getMargin(), getMargin(), getMargin());
    extent.add(margin);

    aabbMin.set(center).sub(extent);
    aabbMax.set(center).add(extent);
    stack.leave();
}
项目:libgdx-jbullet    文件:CapsuleShape.java   
@Override
public void getAabb (Transform t, Vector3 aabbMin, Vector3 aabbMax) {
    Stack stack = Stack.enter();
    Vector3 tmp = stack.allocVector3();

    Vector3 halfExtents = stack.allocVector3();
    halfExtents.set(getRadius(), getRadius(), getRadius());
    VectorUtil.setCoord(halfExtents, upAxis, getRadius() + getHalfHeight());

    halfExtents.x += getMargin();
    halfExtents.y += getMargin();
    halfExtents.z += getMargin();

    Matrix3 abs_b = stack.allocMatrix3();
    abs_b.set(t.basis);
    MatrixUtil.absolute(abs_b);

    Vector3 center = t.origin;
    Vector3 extent = stack.allocVector3();

    MatrixUtil.getRow(abs_b, 0, tmp);
    extent.x = tmp.dot(halfExtents);
    MatrixUtil.getRow(abs_b, 1, tmp);
    extent.y = tmp.dot(halfExtents);
    MatrixUtil.getRow(abs_b, 2, tmp);
    extent.z = tmp.dot(halfExtents);

    aabbMin.set(center).sub(extent);
    aabbMax.set(center).add(extent);
    stack.leave();
}
项目:libgdx-jbullet    文件:GjkEpaSolver.java   
public void init (/* StackAlloc psa, */
    Matrix3 wrot0, Vector3 pos0, ConvexShape shape0, Matrix3 wrot1, Vector3 pos1, ConvexShape shape1, float pmargin) {
    pushStack();
    wrotations[0].set(wrot0);
    positions[0].set(pos0);
    shapes[0] = shape0;
    wrotations[1].set(wrot1);
    positions[1].set(pos1);
    shapes[1] = shape1;
    // sa =psa;
    // sablock =sa->beginBlock();
    margin = pmargin;
    failed = false;
}
项目:libgdx-jbullet    文件:TransformUtil.java   
public static void calculateDiffAxisAngle (Transform transform0, Transform transform1, Vector3 axis, float[] angle) {
// #ifdef USE_QUATERNION_DIFF
// btQuaternion orn0 = transform0.getRotation();
// btQuaternion orn1a = transform1.getRotation();
// btQuaternion orn1 = orn0.farthest(orn1a);
// btQuaternion dorn = orn1 * orn0.inverse();
// #else
        Stack stack = Stack.enter();
        Matrix3 tmp = stack.allocMatrix3();
        tmp.set(transform0.basis);
        MatrixUtil.invert(tmp);

        Matrix3 dmat = stack.allocMatrix3();
        dmat.set(transform1.basis).mul(tmp);

        Quaternion dorn = stack.allocQuaternion();
        MatrixUtil.getRotation(dmat, dorn);
// #endif

        // floating point inaccuracy can lead to w component > 1..., which breaks

        dorn.nor();

        angle[0] = QuaternionUtil.getAngle(dorn);
        axis.set(dorn.x, dorn.y, dorn.z);
        // TODO: probably not needed
        // axis[3] = btScalar(0.);

        // check for axis length
        float len = axis.len2();
        if (len < BulletGlobals.FLT_EPSILON * BulletGlobals.FLT_EPSILON) {
            axis.set(1f, 0f, 0f);
        } else {
            axis.scl(1f / (float)Math.sqrt(len));
        }
        stack.leave();
    }
项目:libgdx-jbullet    文件:Transform.java   
public void set(Matrix4 mat) {
    basis.val[Matrix3.M00] = mat.val[Matrix4.M00];
    basis.val[Matrix3.M01] = mat.val[Matrix4.M01];
    basis.val[Matrix3.M02] = mat.val[Matrix4.M02];
    basis.val[Matrix3.M10] = mat.val[Matrix4.M10];
    basis.val[Matrix3.M11] = mat.val[Matrix4.M11];
    basis.val[Matrix3.M12] = mat.val[Matrix4.M12];
    basis.val[Matrix3.M20] = mat.val[Matrix4.M20];
    basis.val[Matrix3.M21] = mat.val[Matrix4.M21];
    basis.val[Matrix3.M22] = mat.val[Matrix4.M22];
    origin.set(mat.val[Matrix4.M03], mat.val[Matrix4.M13], mat.val[Matrix4.M23]);
}
项目:libgdx-jbullet    文件:Transform.java   
public void invXform(Vector3 inVec, Vector3 out) {
    out.set(inVec).sub(origin);
    Stack stack = Stack.enter();
    Matrix3 mat = stack.alloc(basis);
    mat.transpose();
    out.mul(mat);
    stack.leave();
}
项目:libgdx-jbullet    文件:MatrixUtil.java   
public static void add (Matrix3 a, Matrix3 b) {
    a.val[Matrix3.M00] += b.val[Matrix3.M00];
    a.val[Matrix3.M01] += b.val[Matrix3.M01];
    a.val[Matrix3.M02] += b.val[Matrix3.M02];
    a.val[Matrix3.M10] += b.val[Matrix3.M10];
    a.val[Matrix3.M11] += b.val[Matrix3.M11];
    a.val[Matrix3.M12] += b.val[Matrix3.M12];
    a.val[Matrix3.M20] += b.val[Matrix3.M20];
    a.val[Matrix3.M21] += b.val[Matrix3.M21];
    a.val[Matrix3.M22] += b.val[Matrix3.M22];
}
项目:libgdx-jbullet    文件:MatrixUtil.java   
public static void scale (Matrix3 dest, Matrix3 mat, Vector3 s) {
    dest.val[Matrix3.M00] = mat.val[Matrix3.M00] * s.x;
    dest.val[Matrix3.M01] = mat.val[Matrix3.M01] * s.y;
    dest.val[Matrix3.M02] = mat.val[Matrix3.M02] * s.z;
    dest.val[Matrix3.M10] = mat.val[Matrix3.M10] * s.x;
    dest.val[Matrix3.M11] = mat.val[Matrix3.M11] * s.y;
    dest.val[Matrix3.M12] = mat.val[Matrix3.M12] * s.z;
    dest.val[Matrix3.M20] = mat.val[Matrix3.M20] * s.x;
    dest.val[Matrix3.M21] = mat.val[Matrix3.M21] * s.y;
    dest.val[Matrix3.M22] = mat.val[Matrix3.M22] * s.z;
}
项目:libgdx-jbullet    文件:MatrixUtil.java   
public static void absolute (Matrix3 mat) {
    mat.val[Matrix3.M00] = Math.abs(mat.val[Matrix3.M00]);
    mat.val[Matrix3.M01] = Math.abs(mat.val[Matrix3.M01]);
    mat.val[Matrix3.M02] = Math.abs(mat.val[Matrix3.M02]);
    mat.val[Matrix3.M10] = Math.abs(mat.val[Matrix3.M10]);
    mat.val[Matrix3.M11] = Math.abs(mat.val[Matrix3.M11]);
    mat.val[Matrix3.M12] = Math.abs(mat.val[Matrix3.M12]);
    mat.val[Matrix3.M20] = Math.abs(mat.val[Matrix3.M20]);
    mat.val[Matrix3.M21] = Math.abs(mat.val[Matrix3.M21]);
    mat.val[Matrix3.M22] = Math.abs(mat.val[Matrix3.M22]);
}
项目:libgdx-jbullet    文件:MatrixUtil.java   
public static void setFromOpenGLSubMatrix (Matrix3 mat, float[] m) {
    mat.val[Matrix3.M00] = m[0];
    mat.val[Matrix3.M01] = m[4];
    mat.val[Matrix3.M02] = m[8];
    mat.val[Matrix3.M10] = m[1];
    mat.val[Matrix3.M11] = m[5];
    mat.val[Matrix3.M12] = m[9];
    mat.val[Matrix3.M20] = m[2];
    mat.val[Matrix3.M21] = m[6];
    mat.val[Matrix3.M22] = m[10];
}
项目:libgdx-jbullet    文件:MatrixUtil.java   
public static void getOpenGLSubMatrix (Matrix3 mat, float[] m) {
    m[0] = mat.val[Matrix3.M00];
    m[1] = mat.val[Matrix3.M10];
    m[2] = mat.val[Matrix3.M20];
    m[3] = 0f;
    m[4] = mat.val[Matrix3.M01];
    m[5] = mat.val[Matrix3.M11];
    m[6] = mat.val[Matrix3.M21];
    m[7] = 0f;
    m[8] = mat.val[Matrix3.M02];
    m[9] = mat.val[Matrix3.M12];
    m[10] = mat.val[Matrix3.M22];
    m[11] = 0f;
}
项目:libgdx-jbullet    文件:MatrixUtil.java   
public static void transposeTransform (Vector3 dest, Vector3 vec, Matrix3 mat) {
    float x = tdotx(mat, vec);
    float y = tdoty(mat, vec);
    float z = tdotz(mat, vec);
    dest.x = x;
    dest.y = y;
    dest.z = z;
}
项目:libgdx-jbullet    文件:MatrixUtil.java   
public static void invert (Matrix3 mat) {
    float co_x = cofac(mat, 1, 1, 2, 2);
    float co_y = cofac(mat, 1, 2, 2, 0);
    float co_z = cofac(mat, 1, 0, 2, 1);

    float det = mat.val[Matrix3.M00] * co_x + mat.val[Matrix3.M01] * co_y + mat.val[Matrix3.M02] * co_z;
    assert (det != 0f);

    float s = 1f / det;
    float m00 = co_x * s;
    float m01 = cofac(mat, 0, 2, 2, 1) * s;
    float m02 = cofac(mat, 0, 1, 1, 2) * s;
    float m10 = co_y * s;
    float m11 = cofac(mat, 0, 0, 2, 2) * s;
    float m12 = cofac(mat, 0, 2, 1, 0) * s;
    float m20 = co_z * s;
    float m21 = cofac(mat, 0, 1, 2, 0) * s;
    float m22 = cofac(mat, 0, 0, 1, 1) * s;

    mat.val[Matrix3.M00] = m00;
    mat.val[Matrix3.M01] = m01;
    mat.val[Matrix3.M02] = m02;
    mat.val[Matrix3.M10] = m10;
    mat.val[Matrix3.M11] = m11;
    mat.val[Matrix3.M12] = m12;
    mat.val[Matrix3.M20] = m20;
    mat.val[Matrix3.M21] = m21;
    mat.val[Matrix3.M22] = m22;
}
项目:libgdx-jbullet    文件:AabbUtil2.java   
public static void transformAabb (Vector3 halfExtents, float margin, Transform t, Vector3 aabbMinOut, Vector3 aabbMaxOut) {
    Stack stack = Stack.enter();
    Vector3 halfExtentsWithMargin = stack.allocVector3();
    halfExtentsWithMargin.x = halfExtents.x + margin;
    halfExtentsWithMargin.y = halfExtents.y + margin;
    halfExtentsWithMargin.z = halfExtents.z + margin;

    Matrix3 abs_b = stack.alloc(t.basis);
    MatrixUtil.absolute(abs_b);

    Vector3 tmp = stack.allocVector3();

    Vector3 center = stack.alloc(t.origin);
    Vector3 extent = stack.allocVector3();
    tmp.x = abs_b.val[Matrix3.M00];
    tmp.y = abs_b.val[Matrix3.M01];
    tmp.z = abs_b.val[Matrix3.M02];
    extent.x = tmp.dot(halfExtentsWithMargin);
    tmp.x = abs_b.val[Matrix3.M10];
    tmp.y = abs_b.val[Matrix3.M11];
    tmp.z = abs_b.val[Matrix3.M12];
    extent.y = tmp.dot(halfExtentsWithMargin);
    tmp.x = abs_b.val[Matrix3.M20];
    tmp.y = abs_b.val[Matrix3.M21];
    tmp.z = abs_b.val[Matrix3.M22];
    extent.z = tmp.dot(halfExtentsWithMargin);

    aabbMinOut.set(center).sub(extent);
    aabbMaxOut.set(center).add(extent);
    stack.leave();
}
项目:libgdx-jbullet    文件:Stack.java   
public Matrix3 allocMatrix3() {
    types[sp++] = TYPE_MATRIX3;
    int pos = stackPositions[TYPE_MATRIX3]++;
    if (matrix3fStack.size() <= pos) {
        matrix3fStack.add(new Matrix3());
    }
    return matrix3fStack.get(pos);
}
项目:libgdx-jbullet    文件:RigidBody.java   
public void updateInertiaTensor () {
    Stack stack = Stack.enter();
    Matrix3 mat1 = stack.allocMatrix3();
    MatrixUtil.scale(mat1, worldTransform.basis, invInertiaLocal);

    Matrix3 mat2 = stack.alloc(worldTransform.basis);
    mat2.transpose();

    invInertiaTensorWorld.set(mat1).mul(mat2);
    stack.leave();
}
项目:libgdx-jbullet    文件:Point2PointConstraint.java   
@Override
public void buildJacobian () {
    appliedImpulse = 0f;
    Stack stack = Stack.enter();

    Vector3 normal = stack.allocVector3();
    normal.set(0f, 0f, 0f);

    Matrix3 tmpMat1 = stack.allocMatrix3();
    Matrix3 tmpMat2 = stack.allocMatrix3();
    Vector3 tmp1 = stack.allocVector3();
    Vector3 tmp2 = stack.allocVector3();
    Vector3 tmpVec = stack.allocVector3();

    Transform centerOfMassA = rbA.getCenterOfMassTransform(stack.allocTransform());
    Transform centerOfMassB = rbB.getCenterOfMassTransform(stack.allocTransform());

    for (int i = 0; i < 3; i++) {
        VectorUtil.setCoord(normal, i, 1f);

        tmpMat1.set(centerOfMassA.basis).transpose();
        tmpMat2.set(centerOfMassB.basis).transpose();

        tmp1.set(pivotInA);
        centerOfMassA.transform(tmp1);
        tmp1.sub(rbA.getCenterOfMassPosition(tmpVec));

        tmp2.set(pivotInB);
        centerOfMassB.transform(tmp2);
        tmp2.sub(rbB.getCenterOfMassPosition(tmpVec));

        jac[i].init(tmpMat1, tmpMat2, tmp1, tmp2, normal, rbA.getInvInertiaDiagLocal(stack.allocVector3()), rbA.getInvMass(),
            rbB.getInvInertiaDiagLocal(stack.allocVector3()), rbB.getInvMass());
        VectorUtil.setCoord(normal, i, 0f);
    }
    stack.leave();
}
项目:libgdx-jbullet    文件:HingeConstraint.java   
public HingeConstraint (RigidBody rbA, Transform rbAFrame) {
    super(TypedConstraintType.HINGE_CONSTRAINT_TYPE, rbA);
    this.rbAFrame.set(rbAFrame);
    this.rbBFrame.set(rbAFrame);
    angularOnly = false;
    enableAngularMotor = false;
    Stack stack = Stack.enter();

    // not providing rigidbody B means implicitly using worldspace for body B

    // flip axis
    this.rbBFrame.basis.val[Matrix3.M02] *= -1f;
    this.rbBFrame.basis.val[Matrix3.M12] *= -1f;
    this.rbBFrame.basis.val[Matrix3.M22] *= -1f;

    this.rbBFrame.origin.set(this.rbAFrame.origin);
    rbA.getCenterOfMassTransform(stack.allocTransform()).transform(this.rbBFrame.origin);

    // start with free
    lowerLimit = 1e30f;
    upperLimit = -1e30f;
    biasFactor = 0.3f;
    relaxationFactor = 1.0f;
    limitSoftness = 0.9f;
    solveLimit = false;
    stack.leave();
}
项目:libgdx-jbullet    文件:Generic6DofConstraint.java   
/** MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html */
private static boolean matrixToEulerXYZ (Matrix3 mat, Vector3 xyz) {
    // // rot = cy*cz -cy*sz sy
    // // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx
    // // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy
    //

    if (getMatrixElem(mat, 2) < 1.0f) {
        if (getMatrixElem(mat, 2) > -1.0f) {
            xyz.x = (float)Math.atan2(-getMatrixElem(mat, 5), getMatrixElem(mat, 8));
            xyz.y = (float)Math.asin(getMatrixElem(mat, 2));
            xyz.z = (float)Math.atan2(-getMatrixElem(mat, 1), getMatrixElem(mat, 0));
            return true;
        } else {
            // WARNING. Not unique. XA - ZA = -atan2(r10,r11)
            xyz.x = -(float)Math.atan2(getMatrixElem(mat, 3), getMatrixElem(mat, 4));
            xyz.y = -BulletGlobals.SIMD_HALF_PI;
            xyz.z = 0.0f;
            return false;
        }
    } else {
        // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11)
        xyz.x = (float)Math.atan2(getMatrixElem(mat, 3), getMatrixElem(mat, 4));
        xyz.y = BulletGlobals.SIMD_HALF_PI;
        xyz.z = 0.0f;
    }

    return false;
}
项目:libgdx-jbullet    文件:Generic6DofConstraint.java   
protected void buildAngularJacobian (/* JacobianEntry jacAngular */int jacAngular_index, Vector3 jointAxisW) {
    Stack stack = Stack.enter();
    Matrix3 mat1 = rbA.getCenterOfMassTransform(stack.allocTransform()).basis;
    mat1.transpose();

    Matrix3 mat2 = rbB.getCenterOfMassTransform(stack.allocTransform()).basis;
    mat2.transpose();

    jacAng[jacAngular_index].init(jointAxisW, mat1, mat2, rbA.getInvInertiaDiagLocal(stack.allocVector3()),
        rbB.getInvInertiaDiagLocal(stack.allocVector3()));
    stack.leave();
}
项目:exterminate    文件:Filter.java   
protected T setParams (Parameter param, Matrix3 value) {
    if (!programBegan) {
        programBegan = true;
        program.begin();
    }
    program.setUniformMatrix(param.mnemonic(), value);
    return (T)this;
}