Adding closestPoint to Colliders.

h3d/col/Bounds.hx

@@ -393,6 +393,11 @@ class Bounds extends Collider {
 		return Math.max(xSize, Math.max(ySize, zSize));
 	}
 
+	public function closestPoint( p : h3d.col.Point ) {
+		throw "Not implemented";
+		return new h3d.col.Point();
+	}
+	
 	public static inline function fromPoints( min : Point, max : Point ) {
 		var b = new Bounds();
 		b.setMin(min);

h3d/col/Capsule.hx

@@ -109,6 +109,11 @@ class Capsule extends Collider {
 		return a.distance(b) + 2 * r;
 	}
 
+	public function closestPoint(p : Point) {
+		throw "not implemented";
+		return new h3d.col.Point();
+	}
+
 	#if !macro
 	public function makeDebugObj() : h3d.scene.Object {
 		var obj = new h3d.scene.Object();

h3d/col/Collider.hx

@@ -11,6 +11,7 @@ abstract class Collider {
 	public abstract function inFrustum( f : Frustum, ?localMatrix : h3d.Matrix ) : Bool;
 	public abstract function inSphere( s : Sphere ) : Bool;
 	public abstract function dimension() : Float;
+	public abstract function closestPoint( p : Point) : Point;
 
 	#if !macro
 	public abstract function makeDebugObj() : h3d.scene.Object;
@@ -55,6 +56,10 @@ class OptimizedCollider extends Collider {
 		return Math.max(a.dimension(), b.dimension());
 	}
 
+	public function closestPoint( p : h3d.col.Point ) {
+		return b.closestPoint(p);
+	}
+
 	#if !macro
 	public function makeDebugObj() : h3d.scene.Object {
 		var bobj = b.makeDebugObj();
@@ -120,6 +125,21 @@ class GroupCollider extends Collider {
 		}
 		return d;
 	}
+
+	public function closestPoint( p : h3d.col.Point ) {
+		var result = null;
+		var lengthSq = Math.POSITIVE_INFINITY;
+		for ( c in colliders ) {
+			var closest = c.closestPoint(p);
+			var lSq = closest.distanceSq(p);
+			if ( lSq < lengthSq ) {
+				result = closest;
+				lengthSq = lSq;
+			}
+		}
+		return result;
+	}
+
 	#if !macro
 	public function makeDebugObj() : h3d.scene.Object {
 		var ret : h3d.scene.Object = null;

h3d/col/FPoint.hx

@@ -35,6 +35,27 @@ class FPoint {
 		return x * p.x + y * p.y + z * p.z;
 	}
 
+	public inline function distanceSq( v : FPoint ) {
+		var dx = v.x - x;
+		var dy = v.y - y;
+		var dz = v.z - z;
+		return dx * dx + dy * dy + dz * dz;
+	}
+
+	public inline function lengthSq() {
+		return x * x + y * y + z * z;
+	}
+
+	public inline function normalized() {
+		var k = lengthSq();
+		if ( k < hxd.Math.EPSILON2 ) k = 0 else k = k.invSqrt();
+		return new FPoint(x * k, y * k, z * k);
+	}
+
+	public inline function scaled(v : Float) {
+		return new FPoint(x * v, y * v, z * v);
+	}
+
 	public function toString() {
 		return 'FPoint{${x.fmt()},${y.fmt()},${z.fmt()}}';
 	}

h3d/col/ObjectCollider.hx

@@ -69,6 +69,11 @@ class ObjectCollider extends Collider {
 		return collider.dimension() * Math.max(Math.max(scale.x, scale.y), scale.z);
 	}
 
+	public function closestPoint( p : h3d.col.Point ) {
+		throw "Not implemented";
+		return new h3d.col.Point();
+	}
+
 	#if !macro
 	public function makeDebugObj() : h3d.scene.Object {
 		var ret = collider.makeDebugObj();

h3d/col/OrientedBounds.hx

@@ -327,6 +327,11 @@ class OrientedBounds extends Collider {
 		return out;
 	}
 
+	public function closestPoint(p : Point) {
+		throw "not implemented";
+		return new Point();
+	}
+
 	public function makeDebugObj():h3d.scene.Object {
 		var g = new h3d.scene.Graphics();
 

h3d/col/Polygon.hx

@@ -113,6 +113,46 @@ class TriPlane extends Collider {
 		return false;
 	}
 
+	public function closestPoint( p : Point ) {
+		var p0 = new Point(p0x, p0y, p0z);
+
+		if ( isPointInTriangle(p.x, p.y, p.z) ) {
+			var d = p.sub(p0);
+			var n = new Point(nx, ny, nz);
+			n.normalize();
+			var dProj = d.sub(n.scaled(d.dot(n)));
+			return p0.add(dProj);
+		}
+		
+		inline function closestPointLine(start : Point, d : Point) {
+			var t = p.sub(start).dot(d) / d.dot(d);
+			t = hxd.Math.clamp(t);
+			return start.add(d.scaled(t));
+		}
+		var d1 = new Point(d1x, d1y, d1z);
+		var d2 = new Point(d2x, d2y, d2z);
+		var p1 = new Point(p0x, p0y, p0z).add(d1);
+		var c1 = closestPointLine(p0, d1);
+		var c2 = closestPointLine(p1, d2.sub(d1));
+		var c3 = closestPointLine(p0, d2);
+
+		var mag1 = p.sub(c1).lengthSq();
+		var mag2 = p.sub(c2).lengthSq();
+		var mag3 = p.sub(c3).lengthSq();
+
+		var min = mag1;
+		var c = c1;
+		if ( mag2 < min ) {
+			min = mag2;
+			c = c2;
+		}
+		if ( mag3 < min ) {
+			min = mag3;
+			c = c3;
+		}
+		return c;
+	}
+
 	inline public function rayIntersection( r : Ray, bestMatch : Bool ) @:privateAccess {
 		var dr = r.lx * nx + r.ly * ny + r.lz * nz;
 		if( dr >= 0 && oriented ) // backface culling
@@ -274,6 +314,22 @@ class Polygon extends Collider {
 		return false;
 	}
 
+	public function closestPoint( p : h3d.col.Point ) {
+		var t = triPlanes;
+		var minDistSq = hxd.Math.POSITIVE_INFINITY;
+		var closest = null;
+		while( t != null ) {
+			var c = t.closestPoint(p);
+			var distSq = p.distanceSq(c);
+			if ( distSq < minDistSq ) {
+				minDistSq = distSq;
+				closest = c;
+			}
+			t = t.next;
+		}
+		return closest;
+	}
+
 	inline public function dimension() {
 		return getBounds().dimension();
 	}

h3d/col/PolygonBuffer.hx

@@ -88,6 +88,82 @@ class PolygonBuffer extends Collider {
 		return getBounds().dimension();
 	}
 
+	inline function isPointInTriangle( d : FPoint, d1 : FPoint, d2 : FPoint ) {
+		// Compute vectors
+		var dot02 = d1.dot(d);
+		var dot12 = d2.dot(d);
+
+		var dot00 = d1.dot(d1);
+		var dot01 = d1.dot(d2);
+		var dot11 = d2.dot(d2);
+		var invDenom = 1 / (dot00 * dot11 - dot01 * dot01);
+
+		// Compute barycentric coordinates
+		var u = (dot11 * dot02 - dot01 * dot12) * invDenom;
+		var v = (dot00 * dot12 - dot01 * dot02) * invDenom;
+
+		// Check if point is in triangle
+		return (u >= 0) && (v >= 0) && (u + v < 1);
+	}
+
+	inline function closestPointLine(p : FPoint, start : FPoint, d : FPoint) {
+		var t = p.sub(start).dot(d) / d.dot(d);
+		t = hxd.Math.clamp(t);
+		return start.add(d.scaled(t));
+	}
+
+	public function closestPoint( p : h3d.col.Point ) {
+		var p = new FPoint(p.x, p.y, p.z);
+		var minDistSq = hxd.Math.POSITIVE_INFINITY;
+		var closest = null;
+
+		var i = startIndex;
+		for ( t in 0...triCount ) {
+			var i0 = indexes[i++] * 3;
+			var p0 = new FPoint(buffer[i0++], buffer[i0++], buffer[i0]);
+			var i1 = indexes[i++] * 3;
+			var p1 = new FPoint(buffer[i1++], buffer[i1++], buffer[i1]);
+			var i2 = indexes[i++] * 3;
+			var p2 = new FPoint(buffer[i2++], buffer[i2++], buffer[i2]);
+
+			var c = null;
+			var d1 = p1.sub(p0);
+			var d2 = p2.sub(p0);
+			var d = p.sub(p0);
+			if ( isPointInTriangle(d, d1, d2) ) {
+				var n = d1.cross(d2).normalized();
+				var dProj = d.sub(n.scaled(d.dot(n)));
+				c = p0.add(dProj);
+			} else {
+				var c1 = closestPointLine(p, p0, d1);
+				var c2 = closestPointLine(p, p1, d2.sub(d1));
+				var c3 = closestPointLine(p, p0, d2);
+
+				var mag1 = p.sub(c1).lengthSq();
+				var mag2 = p.sub(c2).lengthSq();
+				var mag3 = p.sub(c3).lengthSq();
+
+				var min = mag1;
+				c = c1;
+				if ( mag2 < min ) {
+					min = mag2;
+					c = c2;
+				}
+				if ( mag3 < min ) {
+					min = mag3;
+					c = c3;
+				}
+			}
+			
+			var distSq = p.distanceSq(c);
+			if ( distSq < minDistSq ) {
+				closest = c;
+				minDistSq = distSq;
+			}
+		}
+		return new h3d.col.Point(closest.x, closest.y, closest.z);
+	}
+
 	// Möller–Trumbore intersection
 	public function rayIntersection( r : Ray, bestMatch : Bool ) : Float {
 		var i = startIndex;

h3d/col/SkinCollider.hx

@@ -95,6 +95,11 @@ class SkinCollider extends Collider {
 		}
 	}
 
+	public function closestPoint( p : h3d.col.Point ) {
+		throw "Not implemented";
+		return new h3d.col.Point();
+	}
+
 	#if !macro
 	public function makeDebugObj() : h3d.scene.Object {
 		return new SkinColliderDebugObj(this);

h3d/col/Sphere.hx

@@ -101,6 +101,11 @@ class Sphere extends Collider {
 		return r;
 	}
 
+	public inline function closestPoint( p : h3d.col.Point ) {
+		var d = p.sub(getCenter()).normalized().scaled(r);
+		return d.add(getCenter());
+	}
+
 	public inline function clone() {
 		var s = new Sphere();
 		s.x = x;

h3d/col/TransformCollider.hx

@@ -84,6 +84,14 @@ class TransformCollider extends Collider {
 		return collider.dimension() * scaleMax;
 	}
 
+	public function closestPoint(p : Point) {
+		var localp = p.clone();
+		localp.transform(invMat);
+		var c = collider.closestPoint(localp);
+		c.transform(mat);
+		return c;
+	}
+
 	#if !macro
 	public function makeDebugObj() : h3d.scene.Object {
 		var obj = collider.makeDebugObj();