/**
* Draws the triangle defined through the given values on the renderPanel.
*
* @param positions
* @param colors
* @param normals
*/
private void rasterizeTriangle(
Point4f[] positions, Color3f[] colors, Point2f[] texCoords, Material material) {
Matrix3f alphabetagamma = new Matrix3f();
// start with smallest "possible" bounding rectangle
Point topLeft = new Point(width - 1, height - 1);
Point botRight = new Point(0, 0);
float[] oneOverWarray = new float[3];
for (int i = 0; i < 3; i++) {
minimizeBoundingRectangle(topLeft, botRight, positions[i]);
float[] row = {positions[i].x, positions[i].y, positions[i].w};
alphabetagamma.setRow(i, row);
oneOverWarray[i] = 1 / positions[i].w;
}
alphabetagamma.invert();
validifyBoundingRectangle(topLeft, botRight);
for (int y = topLeft.y; y <= botRight.y; y++) {
for (int x = topLeft.x; x <= botRight.x; x++) {
Vector3f vertexWeights = getVertexWeights(x, y, alphabetagamma);
if (vertexWeights != null) {
float zvalue = new Vector3f(oneOverWarray).dot(vertexWeights);
if (betterZvalue(x, y, zvalue)) {
zBuffer[x][y] = zvalue;
int c;
if (material != null && material.getTexture() != null)
c = interpolateColorFromTexture(vertexWeights, texCoords, material.getTexture());
else c = interpolateColor(vertexWeights, colors).getRGB();
drawPointAt(x, y, c);
}
}
}
}
}
public Vector3f velocityLocal() {
Vector3f v = new Vector3f();
body.getLinearVelocity(v);
Matrix3f invOri = orientation();
invOri.invert();
return invOri.mult(v);
}
@Override
public void setModelViewMatrix(Matrix4f value) {
gl.uniformMatrix(getUniformLocation("matWorldView"), value);
value.get(normalMatrix);
normalMatrix.invert();
normalMatrix.transpose();
gl.uniformMatrix(getUniformLocation("uNormalMatrix"), normalMatrix);
}
public void alignAxisToVec(String axisName, Vector3f vec) {
Vector3f alignAxis = axis(axisName);
Vector3f rotAxis = new Vector3f();
rotAxis.cross(alignAxis, vec);
if (rotAxis.length() == 0) rotAxis = axis(("XYZ".indexOf(axisName) + 1) % 3);
Matrix3f rotMatrix = Matrix3f.rotation(rotAxis, alignAxis.angle(vec));
Matrix3f ori = orientation();
rotMatrix.mul(ori);
orientation(rotMatrix);
}
public void rotateLocal(float x, float y, float z) {
Matrix3f ori = orientation();
Matrix3f rot = new Matrix3f();
MatrixUtil.setEulerZYX(rot, x, y, z);
ori.mul(rot);
orientation(ori);
}
protected void doGroundMovement() {
if (!isOnGround) return;
// ground movement is as follows, the arrow key dictate the target velocity, and accelleration
// is
// added to achieve that. If opposing keys are held the target velocity is set to zero.
// This is the velocity in the normal plane in the player space
Vector3f targetVelocity = new Vector3f();
if (forward) targetVelocity.z += 1;
if (backward) targetVelocity.z -= 1;
if (left) targetVelocity.x -= 1;
if (right) targetVelocity.x += 1;
if (forward != backward || left != right) {
targetVelocity.normalize();
targetVelocity.scale(MAX_SPEED);
}
forward = false;
backward = false;
left = false;
right = false;
Vector3f forward = new Vector3f(0, 0, 1);
QuaternionUtil.quatRotate(owner.getOrientation(), forward, forward);
// right = normal x forward
// forward = right x normal
Vector3f right = new Vector3f();
right.cross(surfaceNormal, forward);
forward.cross(right, surfaceNormal);
right.normalize();
forward.normalize();
Matrix3f transform = new Matrix3f();
transform.setColumn(0, right);
transform.setColumn(1, surfaceNormal);
transform.setColumn(2, forward);
transform.transform(targetVelocity);
Vector3f delta = new Vector3f();
owner.getRigidBody().getLinearVelocity(delta);
delta.sub(targetVelocity, delta);
// feet can't pull and don't need to push for now
// Ds = D - N(N.D)
Vector3f parComp = new Vector3f(surfaceNormal);
parComp.scale(surfaceNormal.dot(delta));
delta.sub(delta, parComp);
owner.getRigidBody().applyCentralForce(scaleForce(delta));
}
/**
* Returns a vector of the edge coefficients or null if one of them is smaller than 0
*
* @param x
* @param y
* @param alphabetagamma
* @return
*/
private Vector3f getVertexWeights(int x, int y, Matrix3f alphabetagamma) {
Vector3f abgVector = new Vector3f();
float[] coeffs = new float[3];
for (int i = 0; i < 3; i++) {
alphabetagamma.getColumn(i, abgVector);
float coeff = abgVector.dot(new Vector3f(x, y, 1));
if (coeff < 0) return null;
else coeffs[i] = coeff;
}
return new Vector3f(coeffs);
}
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);
}
public void setTransform(
float x1, float y1, float z1, float x2, float y2, float z2, float x3, float y3, float z3) {
if (matrixRotate != null || !doSetOrientation) return;
matrixRotate = new Matrix3f();
Vector3f v = new Vector3f();
// rows in Sygress/CAChe and Spartan become columns here
v.set(x1, y1, z1);
v.normalize();
matrixRotate.setColumn(0, v);
v.set(x2, y2, z2);
v.normalize();
matrixRotate.setColumn(1, v);
v.set(x3, y3, z3);
v.normalize();
matrixRotate.setColumn(2, v);
atomSetCollection.setAtomSetCollectionAuxiliaryInfo(
"defaultOrientationMatrix", new Matrix3f(matrixRotate));
// first two matrix column vectors define quaternion X and XY plane
Quaternion q = new Quaternion(matrixRotate);
atomSetCollection.setAtomSetCollectionAuxiliaryInfo("defaultOrientationQuaternion", q);
Logger.info("defaultOrientationMatrix = " + matrixRotate);
}
/**
* Adds a new physics block to be rendered as a rigid body. Translucent blocks are ignored.
*
* @param position The position
* @param type The block type
* @param impulse An impulse
* @param size The size of the block
* @return The created rigid body (if any)
*/
public synchronized BlockRigidBody addBlock(
Vector3f position, byte type, Vector3f impulse, BLOCK_SIZE size, boolean temporary) {
if (temporary && _blocks.size() > MAX_TEMP_BLOCKS) return null;
BoxShape shape = _blockShape;
Block block = BlockManager.getInstance().getBlock(type);
if (block.isTranslucent()) return null;
if (size == BLOCK_SIZE.HALF_SIZE) shape = _blockShapeHalf;
else if (size == BLOCK_SIZE.QUARTER_SIZE) shape = _blockShapeQuarter;
Matrix3f rot = new Matrix3f();
rot.setIdentity();
DefaultMotionState blockMotionState =
new DefaultMotionState(new Transform(new Matrix4f(rot, position, 1.0f)));
Vector3f fallInertia = new Vector3f();
shape.calculateLocalInertia(block.getMass(), fallInertia);
RigidBodyConstructionInfo blockCI =
new RigidBodyConstructionInfo(block.getMass(), blockMotionState, shape, fallInertia);
BlockRigidBody rigidBlock = new BlockRigidBody(blockCI, type);
rigidBlock.setRestitution(0.0f);
rigidBlock.setAngularFactor(0.5f);
rigidBlock.setFriction(0.5f);
rigidBlock._temporary = temporary;
// Apply impulse
rigidBlock.applyImpulse(impulse, new Vector3f(0.0f, 0.0f, 0.0f));
_insertionQueue.add(rigidBlock);
return rigidBlock;
}
RigidBody createBody(
float mass,
Vector3f position,
CollisionShape shape,
Float initialVelocity,
Matrix3f orientation) {
// Create Dynamic Objects
Transform startTransform = new Transform();
startTransform.setIdentity();
// rigidbody is dynamic if and only if mass is non zero,
// otherwise static
boolean isDynamic = (mass != 0f);
Vector3f localInertia = new Vector3f(0, 0, 0);
if (isDynamic) {
shape.calculateLocalInertia(mass, localInertia);
}
if (orientation != null) {
startTransform.set(orientation);
}
startTransform.origin.set(position);
// using motionstate is recommended, it provides
// interpolation capabilities, and only synchronizes
// 'active' objects
DefaultMotionState myMotionState = new DefaultMotionState(startTransform);
RigidBodyConstructionInfo rbInfo =
new RigidBodyConstructionInfo(mass, myMotionState, shape, localInertia);
RigidBody rb = new RigidBody(rbInfo);
rb.setFriction(0.4f);
// rb.setDamping(0.7f, 0.7f);
if (initialVelocity != 0f && orientation != null) {
synchronized (initialVelocityVector) {
initialVelocityVector.set(0f, 0f, initialVelocity);
orientation.transform(initialVelocityVector);
rb.setLinearVelocity(initialVelocityVector);
}
}
return rb;
}
@SuppressWarnings("unchecked")
public static String toJSON(String infoType, Object info) {
// Logger.debug(infoType+" -- "+info);
StringBuilder sb = new StringBuilder();
String sep = "";
if (info == null) return packageJSON(infoType, (String) null);
if (info instanceof Integer || info instanceof Float || info instanceof Double)
return packageJSON(infoType, info.toString());
if (info instanceof String) return packageJSON(infoType, fixString((String) info));
if (info instanceof String[]) {
sb.append("[");
int imax = ((String[]) info).length;
for (int i = 0; i < imax; i++) {
sb.append(sep).append(fixString(((String[]) info)[i]));
sep = ",";
}
sb.append("]");
return packageJSON(infoType, sb);
}
if (info instanceof int[]) {
sb.append("[");
int imax = ((int[]) info).length;
for (int i = 0; i < imax; i++) {
sb.append(sep).append(((int[]) info)[i]);
sep = ",";
}
sb.append("]");
return packageJSON(infoType, sb);
}
if (info instanceof float[]) {
sb.append("[");
int imax = ((float[]) info).length;
for (int i = 0; i < imax; i++) {
sb.append(sep).append(((float[]) info)[i]);
sep = ",";
}
sb.append("]");
return packageJSON(infoType, sb);
}
if (info instanceof double[]) {
sb.append("[");
int imax = ((double[]) info).length;
for (int i = 0; i < imax; i++) {
sb.append(sep).append(((double[]) info)[i]);
sep = ",";
}
sb.append("]");
return packageJSON(infoType, sb);
}
if (info instanceof Point3f[]) {
sb.append("[");
int imax = ((Point3f[]) info).length;
for (int i = 0; i < imax; i++) {
sb.append(sep);
addJsonTuple(sb, ((Point3f[]) info)[i]);
sep = ",";
}
sb.append("]");
return packageJSON(infoType, sb);
}
if (info instanceof String[][]) {
sb.append("[");
int imax = ((String[][]) info).length;
for (int i = 0; i < imax; i++) {
sb.append(sep).append(toJSON(null, ((String[][]) info)[i]));
sep = ",";
}
sb.append("]");
return packageJSON(infoType, sb);
}
if (info instanceof int[][]) {
sb.append("[");
int imax = ((int[][]) info).length;
for (int i = 0; i < imax; i++) {
sb.append(sep).append(toJSON(null, ((int[][]) info)[i]));
sep = ",";
}
sb.append("]");
return packageJSON(infoType, sb);
}
if (info instanceof float[][]) {
sb.append("[");
int imax = ((float[][]) info).length;
for (int i = 0; i < imax; i++) {
sb.append(sep).append(toJSON(null, ((float[][]) info)[i]));
sep = ",";
}
sb.append("]");
return packageJSON(infoType, sb);
}
if (info instanceof float[][][]) {
sb.append("[");
int imax = ((float[][][]) info).length;
for (int i = 0; i < imax; i++) {
sb.append(sep).append(toJSON(null, ((float[][][]) info)[i]));
sep = ",";
}
sb.append("]");
return packageJSON(infoType, sb);
}
if (info instanceof List) {
sb.append("[ ");
int imax = ((List<?>) info).size();
for (int i = 0; i < imax; i++) {
sb.append(sep).append(toJSON(null, ((List<?>) info).get(i)));
sep = ",";
}
sb.append(" ]");
return packageJSON(infoType, sb);
}
if (info instanceof Matrix4f) {
float[] x = new float[4];
Matrix4f m4 = (Matrix4f) info;
sb.append('[');
for (int i = 0; i < 4; i++) {
if (i > 0) sb.append(',');
m4.getRow(i, x);
sb.append(toJSON(null, x));
}
sb.append(']');
return packageJSON(infoType, sb);
}
if (info instanceof Matrix3f) {
float[] x = new float[3];
Matrix3f m3 = (Matrix3f) info;
sb.append('[');
for (int i = 0; i < 3; i++) {
if (i > 0) sb.append(',');
m3.getRow(i, x);
sb.append(toJSON(null, x));
}
sb.append(']');
return packageJSON(infoType, sb);
}
if (info instanceof Tuple3f) {
addJsonTuple(sb, (Tuple3f) info);
return packageJSON(infoType, sb);
}
if (info instanceof AxisAngle4f) {
sb.append("[")
.append(((AxisAngle4f) info).x)
.append(",")
.append(((AxisAngle4f) info).y)
.append(",")
.append(((AxisAngle4f) info).z)
.append(",")
.append((float) (((AxisAngle4f) info).angle * 180d / Math.PI))
.append("]");
return packageJSON(infoType, sb);
}
if (info instanceof Point4f) {
sb.append("[")
.append(((Point4f) info).x)
.append(",")
.append(((Point4f) info).y)
.append(",")
.append(((Point4f) info).z)
.append(",")
.append(((Point4f) info).w)
.append("]");
return packageJSON(infoType, sb);
}
if (info instanceof Map) {
sb.append("{ ");
Iterator<String> e = ((Map<String, ?>) info).keySet().iterator();
while (e.hasNext()) {
String key = e.next();
sb.append(sep).append(packageJSON(key, toJSON(null, ((Map<?, ?>) info).get(key))));
sep = ",";
}
sb.append(" }");
return packageJSON(infoType, sb);
}
return packageJSON(infoType, fixString(info.toString()));
}
@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));
}
}
}
