Skip to content

Instantly share code, notes, and snippets.

@TheMode

TheMode/PhysicsWorld.java

Created August 18, 2024 15:48
Show Gist options

  • Select an option

    Learn more about clone URLs
  • Save TheMode/29485a5e51f3865813f4d97449af32ed to your computer and use it in GitHub Desktop.

Select an option

Learn more about clone URLs
Save TheMode/29485a5e51f3865813f4d97449af32ed to your computer and use it in GitHub Desktop.
Download ZIP
Minestom scratch PhysX
Raw
PhysicsWorld.java
package net.minestom.scratch.world;
import it.unimi.dsi.fastutil.ints.Int2ObjectMap;
import it.unimi.dsi.fastutil.ints.Int2ObjectOpenHashMap;
import net.minestom.server.collision.Aerodynamics;
import net.minestom.server.collision.BoundingBox;
import net.minestom.server.collision.ShapeImpl;
import net.minestom.server.coordinate.CoordConversionUtils;
import net.minestom.server.coordinate.Point;
import net.minestom.server.coordinate.Pos;
import net.minestom.server.coordinate.Vec;
import net.minestom.server.instance.block.Block;
import net.minestom.server.instance.palette.Palette;
import org.jetbrains.annotations.NotNull;
import physx.PxTopLevelFunctions;
import physx.common.*;
import physx.geometry.PxBoxGeometry;
import physx.geometry.PxGeometry;
import physx.geometry.PxSphereGeometry;
import physx.physics.*;
import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
import java.util.Map;
import java.util.function.BiConsumer;
import java.util.function.IntFunction;
public final class PhysicsWorld implements Block.Setter {
public static final int THREADS = 4;
public static final int VERSION;
public static final PxFoundation FOUNDATION;
static {
// get PhysX library version
VERSION = PxTopLevelFunctions.getPHYSICS_VERSION();
final int versionMajor = VERSION >> 24;
final int versionMinor = (VERSION >> 16) & 0xff;
final int versionMicro = (VERSION >> 8) & 0xff;
System.out.printf("PhysX loaded, version: %d.%d.%d\n", versionMajor, versionMinor, versionMicro);
FOUNDATION = PxTopLevelFunctions.CreateFoundation(VERSION, new PxDefaultAllocator(), new PxDefaultErrorCallback());
}
final PxPhysics physics;
final PxScene scene;
final Int2ObjectMap<Entity> entities = new Int2ObjectOpenHashMap<>();
final Map<PxActor, Entity> actorEntityMap = new HashMap<>();
public PhysicsWorld() {
// create PhysX main physics object
PxTolerancesScale tolerances = new PxTolerancesScale();
this.physics = PxTopLevelFunctions.CreatePhysics(VERSION, FOUNDATION, tolerances);
// create the CPU dispatcher, can be shared among multiple scenes
PxDefaultCpuDispatcher cpuDispatcher = PxTopLevelFunctions.DefaultCpuDispatcherCreate(THREADS);
// create a physics scene
PxSceneDesc sceneDesc = new PxSceneDesc(tolerances);
sceneDesc.setCpuDispatcher(cpuDispatcher);
sceneDesc.setFilterShader(PxTopLevelFunctions.DefaultFilterShader());
this.scene = physics.createScene(sceneDesc);
}
public void tick(BiConsumer<Integer, State> consumer) {
final int numSubSteps = 1;
for (int i = 0; i < numSubSteps; i++) {
scene.simulate(1 / (float) numSubSteps / 20f);
scene.fetchResults(true);
}
// Update all entities
for (Entity entity : entities.values()) {
final State state = entity.updatedState();
if (state != null) consumer.accept(entity.id(), state);
}
}
public void gravity(Point point) {
this.scene.setGravity(toPxVec3(point));
}
public void staticBody(Point origin, Shape shape, ShapeMaterial shapeMaterial) {
PxShapeFlags shapeFlags = new PxShapeFlags((byte) (PxShapeFlagEnum.eSCENE_QUERY_SHAPE.value | PxShapeFlagEnum.eSIMULATION_SHAPE.value));
PxShape groundShape = physics.createShape(toPxGeometry(shape), toPxMaterial(physics, shapeMaterial), true, shapeFlags);
var ground = physics.createRigidStatic(transform(origin));
groundShape.setSimulationFilterData(new PxFilterData(1, 1, 0, 0));
ground.attachShape(groundShape);
scene.addActor(ground);
}
public Entity entityId(int id) {
return entities.get(id);
}
public Entity spawn(int id, Pos pos, BoundingBox boundingBox, Aerodynamics aerodynamics) {
var entity = new Entity(id, pos, boundingBox, aerodynamics);
this.entities.put(id, entity);
return entity;
}
private final Map<Vec, Section> sections = new HashMap<>();
public void palette(Palette palette, int sectionX, int sectionY, int sectionZ) {
final Vec vec = new Vec(sectionX, sectionY, sectionZ);
Section section = sections.computeIfAbsent(vec, v -> new Section(sectionX, sectionY, sectionZ));
section.updatePalette(palette);
}
@Override
public void setBlock(int x, int y, int z, @NotNull Block block) {
Vec point = new Vec(x, y, z);
final int sectionX = point.chunkX();
final int sectionY = point.section();
final int sectionZ = point.chunkZ();
final Vec vec = new Vec(sectionX, sectionY, sectionZ);
Section section = sections.computeIfAbsent(vec, v -> new Section(sectionX, sectionY, sectionZ));
section.updateBlock(
CoordConversionUtils.globalToSectionRelative(point.blockX()),
CoordConversionUtils.globalToSectionRelative(point.blockY()),
CoordConversionUtils.globalToSectionRelative(point.blockZ()),
block);
}
private final class Section {
private final int sectionX, sectionY, sectionZ;
private final PxMaterial mat = toPxMaterial(physics, new ShapeMaterial(0.5f, 0.5f, 0f));
Palette palette = Palette.blocks();
List<PxRigidStatic> actors = new ArrayList<>();
public Section(int sectionX, int sectionY, int sectionZ) {
this.sectionX = sectionX;
this.sectionY = sectionY;
this.sectionZ = sectionZ;
}
void updatePalette(Palette palette) {
clearActors();
this.palette = palette;
if (palette.count() > 0) {
this.actors = actors(palette);
register();
}
}
void updateBlock(int x, int y, int z, Block block) {
clearActors();
final int stateId = block.stateId();
this.palette.set(x, y, z, stateId);
this.actors = actors(palette);
register();
}
private void clearActors() {
for (PxRigidStatic actor : actors) {
scene.removeActor(actor);
}
actors.clear();
}
private void register() {
for (PxRigidStatic actor : actors) {
scene.addActor(actor);
}
}
private List<PxRigidStatic> actors(Palette palette) {
List<PxRigidStatic> actors = new ArrayList<>();
// TODO: section meshing
if (palette.count() == 16 * 16 * 16) {
PxRigidStatic actor = filledSection();
actors.add(actor);
return actors;
}
palette.getAllPresent((x, y, z, value) -> {
Block block = Block.fromStateId(value);
assert block != null;
ShapeImpl blockShape = (ShapeImpl) block.registry().collisionShape();
List<BoundingBox> boxes = blockShape.collisionBoundingBoxes();
var actor = registerBlock(x, y, z, boxes);
actors.add(actor);
});
return actors;
}
private PxRigidStatic registerBlock(int x, int y, int z, List<BoundingBox> boxes) {
final Vec middle = new Vec(x + sectionX * 16, y + sectionY * 16, z + sectionZ * 16);
PxRigidStatic actor = physics.createRigidStatic(transform(middle));
for (BoundingBox box : boxes) {
final Shape shape = new Shape.Box((float) box.width() / 2, (float) box.height() / 2, (float) box.width() / 2);
//System.out.println("place " + middle + " " + shape);
PxShapeFlags shapeFlags = new PxShapeFlags((byte) (PxShapeFlagEnum.eSCENE_QUERY_SHAPE.value | PxShapeFlagEnum.eSIMULATION_SHAPE.value));
PxShape boxShape = physics.createShape(toPxGeometry(shape), mat, true, shapeFlags);
{
PxTransform transform = new PxTransform(PxIDENTITYEnum.PxIdentity);
transform.setP(new PxVec3(0.5f, 0.5f, 0.5f));
boxShape.setLocalPose(transform);
}
boxShape.setSimulationFilterData(new PxFilterData(1, 1, 0, 0));
actor.attachShape(boxShape);
}
return actor;
}
private PxRigidStatic filledSection() {
final Vec vec = new Vec(sectionX * 16, sectionY * 16, sectionZ * 16);
final Shape shape = new Shape.Box(8, 8, 8);
PxShapeFlags shapeFlags = new PxShapeFlags((byte) (PxShapeFlagEnum.eSCENE_QUERY_SHAPE.value | PxShapeFlagEnum.eSIMULATION_SHAPE.value));
PxShape boxShape = physics.createShape(toPxGeometry(shape), mat, true, shapeFlags);
boxShape.setSimulationFilterData(new PxFilterData(1, 1, 0, 0));
{
PxTransform transform = new PxTransform(PxIDENTITYEnum.PxIdentity);
transform.setP(new PxVec3(8, 8, 8));
boxShape.setLocalPose(transform);
}
PxRigidStatic actor = physics.createRigidStatic(transform(vec));
actor.attachShape(boxShape);
return actor;
}
}
public void remove(int id) {
Entity entity = entities.remove(id);
if (entity == null) return;
scene.removeActor(entity.body);
actorEntityMap.remove(entity.body);
}
public List<Entity> raycast(Point origin, Point direction, float range) {
PxRaycastResult result = new PxRaycastResult();
scene.raycast(toPxVec3(origin), toPxVec3(direction), range, result);
return collect(result.getNbTouches(), i -> result.getTouch(i).getActor());
}
public List<Entity> collide(Point origin, Shape shape) {
PxOverlapResult result = new PxOverlapResult();
scene.overlap(toPxGeometry(shape), transform(origin), result);
return collect(result.getNbTouches(), i -> result.getTouch(i).getActor());
}
private List<Entity> collect(int count, IntFunction<PxActor> actorIntFunction) {
List<Entity> hitEntities = new ArrayList<>();
for (int i = 0; i < count; i++) {
var actor = actorIntFunction.apply(i);
Entity entity = actorEntityMap.get(actor);
if (entity != null) hitEntities.add(entity);
}
return hitEntities;
}
public sealed interface Shape {
record Box(float x, float y, float z) implements Shape {
}
record Sphere(float radius) implements Shape {
}
}
public record ShapeMaterial(float staticFriction, float dynamicFriction, float restitution) {
}
public final class Entity {
private final int id;
Pos position;
final PxRigidDynamic body;
public int id() {
return id;
}
public void linearPush(Point direction) {
body.setLinearVelocity(toPxVec3(direction));
}
public void gravity(boolean enable) {
body.setActorFlag(PxActorFlagEnum.eDISABLE_GRAVITY, !enable);
}
Entity(int id, Pos pos, BoundingBox boundingBox, Aerodynamics aerodynamics) {
this.id = id;
this.position = pos;
final float heightOffset = (float) boundingBox.height() / 2;
final Shape shape = new Shape.Box((float) boundingBox.width() / 2, (float) boundingBox.height() / 2, (float) boundingBox.depth() / 2);
PxShapeFlags shapeFlags = new PxShapeFlags((byte) (PxShapeFlagEnum.eSCENE_QUERY_SHAPE.value | PxShapeFlagEnum.eSIMULATION_SHAPE.value));
final ShapeMaterial shapeMaterial = new ShapeMaterial(0.5f, 0.5f, 0f);
System.out.println("shape " + shape);
var geometry = toPxGeometry(shape);
//var geometry = new PxCapsuleGeometry((float) boundingBox.width(), (float) boundingBox.height());
PxShape boxShape = physics.createShape(geometry, toPxMaterial(physics, shapeMaterial), true, shapeFlags);
// Rotate the capsule 90 degrees
{
PxTransform transform = new PxTransform(PxIDENTITYEnum.PxIdentity);
transform.setP(new PxVec3(0, heightOffset, 0));
//transform.setQ(new PxQuat((float) Math.PI / 2, 0, 0, 1));
boxShape.setLocalPose(transform);
}
PxFilterData tmpFilterData = new PxFilterData(1, 1, 0, 0);
this.body = physics.createRigidDynamic(transform(pos));
boxShape.setSimulationFilterData(tmpFilterData);
body.attachShape(boxShape);
// Disable rotation
body.setRigidDynamicLockFlag(PxRigidDynamicLockFlagEnum.eLOCK_ANGULAR_X, true);
body.setRigidDynamicLockFlag(PxRigidDynamicLockFlagEnum.eLOCK_ANGULAR_Y, true);
body.setRigidDynamicLockFlag(PxRigidDynamicLockFlagEnum.eLOCK_ANGULAR_Z, true);
//body.setActorFlag(PxActorFlagEnum.eDISABLE_GRAVITY, true);
//body.addForce(toPxVec3(new Vec(0, aerodynamics.gravity(), 0)), PxForceModeEnum.eACCELERATION);
scene.addActor(body);
actorEntityMap.put(body, this);
}
State updatedState() {
var pose = body.getGlobalPose();
var p = pose.getP();
var q = pose.getQ();
var position = new Pos(p.getX(), p.getY(), p.getZ());
var rotation = new float[]{q.getX(), q.getY(), q.getZ(), q.getW()};
var linearVelocity = body.getLinearVelocity();
var velocity = new Vec(linearVelocity.getX(), linearVelocity.getY(), linearVelocity.getZ());
if (position.samePoint(this.position)) {
return null;
}
final boolean onGround = this.position.y() == position.y();
this.position = position;
return new State(position, velocity, rotation, onGround);
}
}
public record State(Pos position, Vec velocity, float[] rotation, boolean onGround) {
}
private static PxGeometry toPxGeometry(Shape shape) {
return switch (shape) {
case Shape.Box box -> new PxBoxGeometry(box.x(), box.y(), box.z());
case Shape.Sphere sphere -> new PxSphereGeometry(sphere.radius());
};
}
private static PxMaterial toPxMaterial(PxPhysics physics, ShapeMaterial shapeMaterial) {
return physics.createMaterial(shapeMaterial.staticFriction(), shapeMaterial.dynamicFriction(), shapeMaterial.restitution());
}
private static PxTransform transform(Point point) {
PxTransform transform = new PxTransform(PxIDENTITYEnum.PxIdentity);
transform.setP(toPxVec3(point));
return transform;
}
private static PxVec3 toPxVec3(Point point) {
return new PxVec3((float) point.x(), (float) point.y(), (float) point.z());
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment