215 lines
12 KiB
Java
215 lines
12 KiB
Java
package electrosphere.entity.state.collidable;
|
|
|
|
import electrosphere.collision.dispatch.CollisionObject;
|
|
import electrosphere.entity.Entity;
|
|
import electrosphere.entity.EntityDataStrings;
|
|
import electrosphere.entity.EntityUtils;
|
|
import electrosphere.entity.state.gravity.GravityTree;
|
|
import electrosphere.entity.types.collision.CollisionObjUtils;
|
|
import electrosphere.entity.types.creature.CreatureUtils;
|
|
import electrosphere.entity.types.debug.DebugVisualizerUtils;
|
|
import electrosphere.entity.types.item.ItemUtils;
|
|
import electrosphere.game.collision.PhysicsUtils;
|
|
import electrosphere.game.collision.collidable.Collidable;
|
|
import electrosphere.game.data.creature.type.CollidableTemplate;
|
|
import electrosphere.main.Globals;
|
|
|
|
import org.joml.Matrix4f;
|
|
import org.joml.Quaterniond;
|
|
import org.joml.Quaternionf;
|
|
import org.joml.Vector3d;
|
|
import org.joml.Vector3f;
|
|
import org.joml.Vector4d;
|
|
import org.joml.Vector4f;
|
|
|
|
/**
|
|
*
|
|
* @author amaterasu
|
|
*/
|
|
public class CollidableTree {
|
|
|
|
Entity parent;
|
|
CollisionObject body;
|
|
Collidable collidable;
|
|
Quaternionf angularVelocity = new Quaternionf(0,0,0,0);
|
|
Vector4d cumulativeTorque = new Vector4d(0,0,0,0);
|
|
|
|
static final float DELTA_T = 0.01f;
|
|
|
|
public CollidableTree(Entity e, Collidable collidable, CollisionObject body){
|
|
parent = e;
|
|
this.collidable = collidable;
|
|
this.body = body;
|
|
}
|
|
|
|
static int incrementer = 0;
|
|
|
|
public void simulate(){
|
|
Vector3d position = EntityUtils.getPosition(parent);
|
|
Quaternionf rotation = EntityUtils.getRotation(parent);
|
|
Matrix4f inverseInertiaTensor = CollisionObjUtils.getInverseInertiaTensor(parent);
|
|
Vector3d offsetVector = new Vector3d();
|
|
Vector3d newPosition = new Vector3d(position);
|
|
javax.vecmath.Matrix4f bodyTransformMatrix;
|
|
//have we hit a terrain impulse?
|
|
boolean hitTerrain = false;
|
|
//handle impulses
|
|
for(Impulse impulse : collidable.getImpulses()){
|
|
// collidable.getImpulses().remove(impulse);
|
|
Vector3d impulseForce = new Vector3d(impulse.getDirection()).mul(impulse.getForce());
|
|
if(impulse.type.matches(Collidable.TYPE_TERRAIN)){
|
|
hitTerrain = true;
|
|
// System.out.println("Impulse force: " + impulseForce);
|
|
// System.out.println("Position: " + position);
|
|
}
|
|
if(impulse.type.matches(Collidable.TYPE_ITEM)){
|
|
if(parent.containsKey(EntityDataStrings.GRAVITY_TREE)){
|
|
((GravityTree)parent.getData(EntityDataStrings.GRAVITY_TREE)).start();
|
|
}
|
|
}
|
|
if(impulse.type.matches(Collidable.TYPE_CREATURE)){
|
|
// System.out.println(System.currentTimeMillis() + " creature hit!");
|
|
if(parent.containsKey(EntityDataStrings.GRAVITY_TREE)){
|
|
((GravityTree)parent.getData(EntityDataStrings.GRAVITY_TREE)).start();
|
|
}
|
|
}
|
|
if(
|
|
impulse.getCollisionPoint().length() > 0 &&
|
|
!Double.isNaN(impulse.getCollisionPoint().x) &&
|
|
!Double.isNaN(impulse.getCollisionPoint().y) &&
|
|
!Double.isNaN(impulse.getCollisionPoint().z) &&
|
|
!Double.isNaN(impulse.getDirection().x) &&
|
|
!Double.isNaN(impulse.getDirection().y) &&
|
|
!Double.isNaN(impulse.getDirection().z) &&
|
|
ItemUtils.isItem(parent)
|
|
){
|
|
// Vector3d collisionPoint = new Vector3d(impulse.getCollisionPoint()).normalize();
|
|
Vector3d collisionPoint = new Vector3d(impulse.getWorldPoint()).sub(position);
|
|
Vector3d forceDir = new Vector3d(impulse.getDirection()).normalize();
|
|
Vector3d torqueVec = new Vector3d(collisionPoint).cross(forceDir).normalize();
|
|
// double torqueMag = Math.abs(impulse.force);
|
|
double torqueMag = Math.sqrt(impulse.getCollisionPoint().length()) * impulse.getForce();
|
|
if(impulse.getType().equals(Collidable.TYPE_TERRAIN)){
|
|
torqueMag = torqueMag * 3;
|
|
}
|
|
torqueMag = torqueMag * 10;
|
|
// } else {
|
|
// torqueMag = 0;
|
|
// }
|
|
// if(impulse.type.matches(Collidable.TYPE_ITEM) && ItemUtils.isItem(parent)){
|
|
// // System.out.println(rotation);
|
|
// if(impulse.collisionPoint.x < 0){
|
|
// // System.out.println("Impulse collision point: " + impulse.getCollisionPoint() + " direction: " + impulse.getDirection() + " => " + new Vector3d(impulse.getCollisionPoint()).cross(impulse.getDirection()));
|
|
// cumulativeTorque.add(new Vector3d(impulse.getCollisionPoint()).cross(impulse.getDirection()));
|
|
// }
|
|
// } else {
|
|
// // angularVelocity.add(new Vector3d(impulse.getCollisionPoint()).cross(impulse.getDirection()),1.0);
|
|
// cumulativeTorque.add(new Vector3d(impulse.getCollisionPoint()).cross(impulse.getDirection()));
|
|
// }
|
|
if(impulse.type.matches(Collidable.TYPE_CREATURE)){
|
|
// System.out.println("Impulse: " + impulse.getCollisionPoint() + " x " + impulse.getDirection() + " ->f " + impulse.force);
|
|
// incrementer++;
|
|
// if(incrementer > 5){
|
|
// Globals.microSimulation.freeze();
|
|
// } else if(incrementer <= 5){
|
|
// // Globals.controlHandler.showMouse();
|
|
// Vector3d pos = impulse.getWorldPoint();
|
|
// // pos = new Vector3d(position).add(impulse.getCollisionPoint()).mul(1,0,1);
|
|
// DebugVisualizerUtils.spawnVectorVisualizer(impulse.getWorldPoint(), new Vector3d(torqueVec));
|
|
// }
|
|
// System.out.println("Impulse: " + torqueVec + " " + torqueMag);
|
|
}
|
|
// if(CreatureUtils.isCreature(parent) && forceDir.y < 0.5){
|
|
// System.out.println(collisionPoint + " x " + forceDir + " => " + torqueVec + " " + torqueMag);
|
|
// }
|
|
Quaternionf impulseRotation = new Quaternionf().rotationAxis((float)torqueMag * DELTA_T,new Vector3f((float)torqueVec.x,(float)torqueVec.y,(float)torqueVec.z));
|
|
if(angularVelocity.lengthSquared() > 0.001){
|
|
angularVelocity.mul(impulseRotation);
|
|
} else {
|
|
angularVelocity.set(impulseRotation);
|
|
}
|
|
// angularVelocity.add(new Vector4d((float)torqueVec.x,(float)torqueVec.y,(float)torqueVec.z,(float)torqueMag));
|
|
// cumulativeTorque.add(new Vector4d((float)torqueVec.x,(float)torqueVec.y,(float)torqueVec.z,(float)torqueMag));
|
|
}
|
|
offsetVector.add(impulseForce);
|
|
}
|
|
// if(ItemUtils.isItem(parent) && cumulativeTorque.w > 0.001){
|
|
// System.out.println(cumulativeTorque);
|
|
// }
|
|
//friction
|
|
if(angularVelocity.lengthSquared() > 0.001){
|
|
angularVelocity.slerp(new Quaternionf(0,0,0,1), 0.03f);
|
|
// angularVelocity.scale((float)(Math.sqrt(angularVelocity.lengthSquared()) * 0.9));
|
|
// System.out.println(angularVelocity);
|
|
}
|
|
// if(cumulativeTorque.w > 0.001){
|
|
// Vector4f holder = inverseInertiaTensor.transform(new Vector4f((float)cumulativeTorque.x,(float)cumulativeTorque.y,(float)cumulativeTorque.z,(float)cumulativeTorque.w));
|
|
// cumulativeTorque = new Vector4d(holder.x,holder.y,holder.z,holder.w);
|
|
// angularVelocity = angularVelocity.add(cumulativeTorque).normalize();
|
|
// cumulativeTorque.set(0,0,0,0);
|
|
// // Vector3d normalizedTorqueDir = new Vector3d(cumulativeTorque.x,cumulativeTorque.y,cumulativeTorque.z).normalize();
|
|
// // double newMag = cumulativeTorque.w * 0.9;
|
|
// // cumulativeTorque.set(normalizedTorqueDir.x,normalizedTorqueDir.y,normalizedTorqueDir.z,newMag);
|
|
// }
|
|
if(angularVelocity.lengthSquared() > 0.001){
|
|
// System.out.println("-" + rotation);
|
|
Quaternionf newRotation = new Quaternionf(rotation).mul(angularVelocity).normalize();
|
|
// if(new Quaternionf(newRotation).add(new Quaternionf(rotation).conjugate()).lengthSquared() > 0.2){
|
|
// newRotation.w = Math.copySign(newRotation.w, rotation.w);
|
|
// newRotation.x = Math.copySign(newRotation.x, rotation.x);
|
|
// newRotation.y = Math.copySign(newRotation.y, rotation.y);
|
|
// newRotation.z = Math.copySign(newRotation.z, rotation.z);
|
|
// }
|
|
rotation.set(newRotation);
|
|
// System.out.println("=" + rotation);
|
|
}
|
|
// if(inverseInertiaTensor != null && angularVelocity.w > 0.01){
|
|
// // Vector4f angularMomentum = inverseInertiaTensor.transform(new Vector4f((float)cumulativeTorque.x,(float)cumulativeTorque.x,(float)cumulativeTorque.x,(float)cumulativeTorque.w));
|
|
// // Quaternionf nextRotation = new Quaternionf(rotation).mul(new Quaternionf(angularMomentum.x,angularMomentum.y,angularMomentum.z,angularMomentum.w).scale(0.001f)).normalize();
|
|
// // rotation = nextRotation;
|
|
// // rotation.mul(new Quaternionf((float)angularMomentum.x,(float)angularMomentum.y,(float)angularMomentum.z,(float)angularMomentum.w / 0.01f));
|
|
// // if(ItemUtils.isItem(parent)){
|
|
// // System.out.println("cumulative quat: " + cumulativeTorque);
|
|
// // }
|
|
// rotation.x = rotation.x + rotation.x * (float)(angularVelocity.x * angularVelocity.w) * 0.01f;
|
|
// rotation.y = rotation.y + rotation.y * (float)(angularVelocity.y * angularVelocity.w) * 0.01f;
|
|
// rotation.z = rotation.z + rotation.z * (float)(angularVelocity.z * angularVelocity.w) * 0.01f;
|
|
// rotation.w = 1;
|
|
// // rotation.w = rotation.w + rotation.w * (float)cumulativeTorque.w * 0.001f;
|
|
// rotation.normalize();
|
|
// }
|
|
|
|
//the reasoning here is that if we have something above something else and it's pushing it into the terrain,
|
|
//we should instead just not push into terrain and push along terrain
|
|
if(hitTerrain && offsetVector.y < 0){
|
|
offsetVector.y = 0;
|
|
}
|
|
|
|
//make sure we're in a valid (World bounds) position
|
|
newPosition.add(offsetVector);
|
|
if(!Globals.collisionEngine.checkCanOccupyPosition(Globals.commonWorldData, parent, newPosition)){
|
|
newPosition = Globals.collisionEngine.suggestMovementPosition(Globals.commonWorldData, parent, newPosition);
|
|
}
|
|
position.set(newPosition);
|
|
|
|
//update collision engine of this thing's position
|
|
CollidableTemplate template = (CollidableTemplate)parent.getData(EntityDataStrings.PHYSICS_MODEL_TEMPLATE);
|
|
bodyTransformMatrix = new javax.vecmath.Matrix4f(PhysicsUtils.jomlToVecmathQuaternionf(rotation),PhysicsUtils.jomlToVecmathVector3f(new Vector3f((float)position.x + template.getOffsetX(),(float)position.y + template.getOffsetY(),(float)position.z + template.getOffsetZ())),1.0f);
|
|
body.setWorldTransform(new electrosphere.linearmath.Transform(bodyTransformMatrix));
|
|
|
|
// bodyTransformMatrix = new javax.vecmath.Matrix4f(PhysicsUtils.jomlToVecmathQuaternionf(rotation),PhysicsUtils.jomlToVecmathVector3f(new Vector3f((float)newPosition.x,(float)newPosition.y,(float)newPosition.z)),1.0f);
|
|
// body.setWorldTransform(new electrosphere.linearmath.Transform(bodyTransformMatrix));
|
|
|
|
}
|
|
|
|
|
|
public void setCollisionObject(CollisionObject body, Collidable collidable){
|
|
this.body = body;
|
|
this.collidable = collidable;
|
|
}
|
|
|
|
|
|
|
|
|
|
}
|