stephengold / lbjexamples Goto Github PK

package engine.math;

import com.jme3.bullet.collision.shapes.BoxCollisionShape;
import com.jme3.bullet.collision.shapes.GImpactCollisionShape;
import com.jme3.bullet.collision.shapes.PlaneCollisionShape;
import com.jme3.bullet.collision.shapes.infos.IndexedMesh;
import com.jme3.bullet.objects.PhysicsRigidBody;
import engine.collision.VkSimOBB;
import engine.collision.VkSimPlane;
import engine.core.VkConcurrentRenderInfo;
import engine.core.VkTransform;
import engine.util.VertexUtil;
import engine.util.VkBulletUtil;

import java.util.Arrays;

public class VkSimModel extends PhysicsRigidBody implements VkSimulate {

    public enum TYPE {PLANE,MESH,BOX};

    private VkModel model;

    private Vector3 velocity = Vector3.ZERO;
    private Vector3 orientation = Vector3.ZERO;
    private Vector3 angularVelocity = Vector3.ZERO;
    private Vector3 position;

    public VkSimModel(VkModel model, float mass) {
        super(new GImpactCollisionShape(
                new IndexedMesh(
                        VkBulletUtil.convert(Arrays.stream(VertexUtil.toVertexArray(model.getVertices()))
                                .map(v -> v.getPos()).toArray(Vector3[]::new)), model.getIndices())), mass);

        this.model = model;
        setPhysicsTransform(VkBulletUtil.convert(model.getLocalTransform()));
        getCollisionShape().setScale(VkBulletUtil.convert(model.getLocalTransform().getScaling()));
        setAngularSleepingThreshold(.001f);
        setLinearSleepingThreshold(.1f);
        setLinearDamping(.98f);
        setAngularDamping(.98f);
    }

    public VkSimModel(VkModel model, VkSimOBB obb, float mass) {

        super(new BoxCollisionShape(obb.getMax().getX() - obb.getCenter().getX(),
                        obb.getMax().getY() - obb.getCenter().getY()
                        ,obb.getMax().getZ() - obb.getCenter().getZ())
                , mass);
        this.model = model;
        setPhysicsTransform(VkBulletUtil.convert(model.getLocalTransform()));
        getCollisionShape().setScale(VkBulletUtil.convert(model.getLocalTransform().getScaling()));
        setAngularSleepingThreshold(.001f);
        setLinearSleepingThreshold(.1f);
        setLinearDamping(.98f);
        setAngularDamping(.98f);
    }

    public VkSimModel(VkModel model, VkSimPlane plane, float mass) {
        super(plane, mass);
        this.model = model;
        setPhysicsTransform(VkBulletUtil.convert(model.getLocalTransform()));
        getCollisionShape().setScale(VkBulletUtil.convert(model.getLocalTransform().getScaling()));
        setAngularSleepingThreshold(.001f);
        setLinearSleepingThreshold(.1f);
        setLinearDamping(.98f);
        setAngularDamping(.98f);
    }

    @Override
    public void update(double elapsedTime) {

        this.position = getPosition().clone();
        this.orientation = getRotation().clone();

        angularVelocity = VkBulletUtil.convert(getAngularVelocity(null))
                .mul(getAngularDamping());

        velocity = VkBulletUtil.convert(getLinearVelocity(null))
                .mul(getLinearDamping());

        // Integrate position
        position = position.add(velocity.mul((float)elapsedTime));

        Quaternion quaternionZ = Quaternion.createFromAxisAngle(Vector3.FRONT, angularVelocity.getZ());
        this.orientation = orientation.add(quaternionZ.normalize().xyz().mul(Quaternion.radToDeg));

        Quaternion quaternionY  = Quaternion.createFromAxisAngle(Vector3.UP, -angularVelocity.getY());
        this.orientation = orientation.add(quaternionY.normalize().xyz().mul(Quaternion.radToDeg));

        Quaternion quaternionX  = Quaternion.createFromAxisAngle(Vector3.RIGHT, -angularVelocity.getX());
        this.orientation = orientation.add(quaternionX.normalize().xyz().mul(Quaternion.radToDeg));


    }


    public VkConcurrentRenderInfo commit() {

        VkTransform transform = new VkTransform();
        transform.setTranslation(this.position);
        transform.setRotation(this.orientation);
        transform.setScaling(this.model.getLocalTransform().getScaling());

        setPhysicsTransform(VkBulletUtil.convert(transform));

        return this.model.updateTransform(transform);
    }

    public Vector3 getRotation() {
        return this.model.getLocalTransform().getRotation();
    }

    public Vector3 getPosition() {
        return this.model.getLocalTransform().getTranslation();
    }

}