@Override public void setModelViewMatrix(Matrix4f value) { gl.uniformMatrix(getUniformLocation("matWorldView"), value); value.get(normalMatrix); normalMatrix.invert(); normalMatrix.transpose(); gl.uniformMatrix(getUniformLocation("uNormalMatrix"), normalMatrix); }
public Vertex bake(Mesh mesh, Function<Node<?>, Matrix4f> animator) { // geometry Float totalWeight = 0f; Matrix4f t = new Matrix4f(); if (mesh.getWeightMap().get(this).isEmpty()) { t.setIdentity(); } else { for (Pair<Float, Node<Bone>> bone : mesh.getWeightMap().get(this)) { totalWeight += bone.getLeft(); Matrix4f bm = animator.apply(bone.getRight()); bm.mul(bone.getLeft()); t.add(bm); } if (Math.abs(totalWeight) > 1e-4) t.mul(1f / totalWeight); else t.setIdentity(); } // pos Vector4f pos = new Vector4f(this.pos), newPos = new Vector4f(); pos.w = 1; t.transform(pos, newPos); Vector3f rPos = new Vector3f(newPos.x / newPos.w, newPos.y / newPos.w, newPos.z / newPos.w); // normal Vector3f rNormal = null; if (this.normal != null) { Matrix3f tm = new Matrix3f(); t.getRotationScale(tm); tm.invert(); tm.transpose(); Vector3f normal = new Vector3f(this.normal); rNormal = new Vector3f(); tm.transform(normal, rNormal); rNormal.normalize(); } // texCoords TODO return new Vertex(rPos, rNormal, color, texCoords); }
@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)); } } }