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; }
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"); } }
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"); } }
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"); } }
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"); } }
/** 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); }
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); }
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); }
/** 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); }
/** 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); }
/** 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); }
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; }
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(); }
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); }
/** * 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; }
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; }
@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(); }
@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(); }
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; }
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(); }
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]); }
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(); }
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]; }
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; }
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]); }
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]; }
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; }
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; }
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; }
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(); }
public Matrix3 allocMatrix3() { types[sp++] = TYPE_MATRIX3; int pos = stackPositions[TYPE_MATRIX3]++; if (matrix3fStack.size() <= pos) { matrix3fStack.add(new Matrix3()); } return matrix3fStack.get(pos); }
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(); }
@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(); }
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(); }
/** 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; }
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(); }
protected T setParams (Parameter param, Matrix3 value) { if (!programBegan) { programBegan = true; program.begin(); } program.setUniformMatrix(param.mnemonic(), value); return (T)this; }