161 lines
8.2 KiB
Java
161 lines
8.2 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.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.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,1);
|
|
Vector4d cumulativeTorque = new Vector4d(0,0,0,0);
|
|
|
|
|
|
|
|
public CollidableTree(Entity e, Collidable collidable, CollisionObject body){
|
|
parent = e;
|
|
this.collidable = collidable;
|
|
this.body = body;
|
|
}
|
|
|
|
|
|
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.getDataKeys().contains(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.getDataKeys().contains(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)
|
|
){
|
|
Vector3d collisionPoint = new Vector3d(impulse.getCollisionPoint()).normalize();
|
|
Vector3d forceDir = new Vector3d(impulse.getDirection()).normalize();
|
|
Vector3d torqueVec = new Vector3d(collisionPoint).cross(forceDir).normalize();
|
|
double torqueMag = Math.abs(impulse.force);
|
|
// double torqueMag = impulse.getCollisionPoint().length() * impulse.getDirection().length();
|
|
// 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_ITEM)){
|
|
// System.out.println("Impulse: " + torqueVec + " " + torqueMag);
|
|
// }
|
|
cumulativeTorque.add(new Vector4d(torqueVec.x,torqueVec.y,torqueVec.z,torqueMag));
|
|
}
|
|
offsetVector.add(impulseForce);
|
|
}
|
|
// if(ItemUtils.isItem(parent) && cumulativeTorque.w > 0.001){
|
|
// System.out.println(cumulativeTorque);
|
|
// }
|
|
//friction
|
|
// if(cumulativeTorque.w > 0.001){
|
|
// 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(inverseInertiaTensor != null && cumulativeTorque.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)cumulativeTorque.x * 0.001f;
|
|
rotation.y = rotation.y + rotation.y * (float)cumulativeTorque.y * 0.001f;
|
|
rotation.z = rotation.z + rotation.z * (float)cumulativeTorque.z * 0.001f;
|
|
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;
|
|
}
|
|
|
|
|
|
|
|
|
|
}
|