DMechPickingDemo.d

import std.stdio;
import std.algorithm;

import dmech.geometry;
import dmech.rigidbody;
import dmech.world;
import dmech.shape;
import dmech.raycast;
import dlib.core.memory;
import dlib.math.vector;
import dlib.math.matrix;
import dlib.math.quaternion;
import dlib.math.transformation;
import dlib.geometry.ray;
import dlib.geometry.aabb;

// Sample picking test, using dmech 0.3.1, dlib 0.12.2
// equivalent bullet physics c++ code at: https://gist.github.com/taumuon/7948d557da1f5779ca98643a433c6e48

void main()
{
	PhysicsWorld world = New!PhysicsWorld(1000);

	uint[RigidBody] rigidBodyMap;
	RigidBody[] rigidBodies;

	for (int x = 0; x < 11; ++x)
	{
		for (int z = 0; z < 11; ++z)
		{
			Geometry gBox = New!(GeomBox)(Vector3f(0.5f, 0.5f, 0.5f)); // Physical shape
			RigidBody bBox = world.addStaticBody(Vector3f(x - 5.0f, 0.0f, z - 5.0f));
			bBox.orientation = Quaternionf(0.0f, 0.0f, 0.0f, 1.0f);
			auto shapeComponent = world.addShapeComponent(bBox, gBox, Vector3f(0.0f,0.0f,0.0f), 1.0f);

			rigidBodyMap[bBox] = (x * 11) + z;
			rigidBodies ~= bBox;
		}
	}

	Vector3f origin = Vector3f(0, 10, 0);
	for (int x = 0; x < 11; ++x)
	{
		for (int z = 0; z < 11; ++z)
		{
			int cube_index = (x * 11 + z);

			CastResult castResult;
			Vector3f target = Vector3f(x - 5, 0, z - 5);
			Vector3f direction = target - origin;
			world.raycast(origin, direction.normalized, 1000, castResult);
			RigidBody rbody = castResult.rbody;

			if (rbody is null)
			{
				writeln("no pick\n");
			}
			else
			{
				writeln("pick for index: ", cube_index, ". result: ",  rigidBodyMap[rbody]);
			}
		}
	}
}