Point Triangle Closest Point 3D
Followup to: http://wonderfl.net/c/b27F
/**
* Copyright mutantleg ( http://wonderfl.net/user/mutantleg )
* MIT License ( http://www.opensource.org/licenses/mit-license.php )
* Downloaded from: http://wonderfl.net/c/mBsK
*/
package {
import flash.text.TextField;
import flash.geom.Vector3D;
import flash.geom.Matrix3D;
import flash.events.Event;
import flash.display.Sprite;
public class FlashTest extends Sprite {
public function FlashTest() {
myTri = new xTri();
myTri.x0 = -40;
myTri.y0 = -40;
myTri.z0 = 5;
myTri.x1 = 50;
myTri.y1 = -10;
myTri.z1 = 0;
myTri.x2 = -20;
myTri.y2 = 30;
myTri.z2 = -3;
myTri.calcNormal();
deb = new TextField();
deb.width = 320;
deb.height = 240;
deb.mouseEnabled = false;
addChild(deb);
stage.addEventListener(Event.ENTER_FRAME, onEnter);
}//ctor
public var deb:TextField;
public var mat:Matrix3D = new Matrix3D();
public var yaw:Number = 0;
public var myTri:xTri;
public var temp:xPnt = new xPnt();
public function onEnter(e:Event):void
{
var raw:Vector.<Number>;
var sx0:Number; var sy0:Number;
var sx1:Number; var sy1:Number;
var sx2:Number; var sy2:Number;
var cx:Number; var cy:Number;
var rx:Number; var ry:Number; var rz:Number;
var px:Number; var py:Number; var pz:Number;
cx = 200;
cy = 200;
px = stage.mouseX - cx;
py = stage.mouseY - cy;
pz = 30;
var pmax:Number;
pmax = 100;
if (px < -pmax) { px = -pmax;}
if (px > pmax) { px= pmax;}
if (py < -pmax) { py = -pmax;}
if (py > pmax) { py= pmax;}
yaw += 2;
mat.identity();
mat.appendRotation(yaw, Vector3D.Y_AXIS);
raw = mat.rawData;
raw[12] = 200;
raw[13] = 200;
raw[14] = 0;
graphics.clear();
graphics.lineStyle(1, 0, 0.5);
graphics.moveTo(cx+200,cy);
graphics.lineTo(cx-200,cy);
graphics.moveTo(cx,cy+200);
graphics.lineTo(cx,cy-200);
// graphics.drawRect(cx-5,cy-5,10,10);
//project triangle
rx = myTri.x0; ry = myTri.y0; rz = myTri.z0;
sx0 = rx*raw[0] + ry*raw[4] + rz*raw[8] + raw[12];
sy0 = rx*raw[1] + ry*raw[5] + rz*raw[9] + raw[13];
rx = myTri.x1; ry = myTri.y1; rz = myTri.z1;
sx1 = rx*raw[0] + ry*raw[4] + rz*raw[8] + raw[12];
sy1 = rx*raw[1] + ry*raw[5] + rz*raw[9] + raw[13];
rx = myTri.x2; ry = myTri.y2; rz = myTri.z2;
sx2 = rx*raw[0] + ry*raw[4] + rz*raw[8] + raw[12];
sy2 = rx*raw[1] + ry*raw[5] + rz*raw[9] + raw[13];
var w:Number;
var c:uint;
w = (sx1 - sx0) * (sy2 - sy0) - (sx2 - sx0) * (sy1 - sy0);
if (w > 0) { c = 0xFF0000; } else { c = 0xFF;}
//draw triangle
graphics.lineStyle(2, 0);
graphics.beginFill(c, 0.2);
graphics.moveTo(sx0, sy0);
graphics.lineTo(sx1, sy1);
graphics.lineTo(sx2, sy2);
graphics.lineTo(sx0, sy0);
graphics.endFill();
//project point
rx = px; ry = py; rz = pz;
sx0 = rx*raw[0] + ry*raw[4] + rz*raw[8] + raw[12];
sy0 = rx*raw[1] + ry*raw[5] + rz*raw[9] + raw[13];
//draw point
graphics.lineStyle(2,0);
graphics.drawCircle(sx0, sy0, 8);
var dist:Number;
dist = myTri.getDist(px,py,pz, temp);
deb.text = "Dist: "+ Math.sqrt(dist);
//project closest point
rx = temp.px; ry = temp.py; rz = temp.pz;
sx1 = rx*raw[0] + ry*raw[4] + rz*raw[8] + raw[12];
sy1 = rx*raw[1] + ry*raw[5] + rz*raw[9] + raw[13];
//draw closest point
graphics.lineStyle(2,0);
// graphics.drawCircle(sx0, sy0, 4);
graphics.drawRect(sx1-4,sy1-4,8,8);
graphics.moveTo(sx0,sy0);
graphics.lineTo(sx1, sy1);
}//onenter
}//classend
}
internal class xPnt
{
public var px:Number = 0;
public var py:Number = 0;
public var pz:Number = 0;
}//xpnt
internal class xTri
{
public var x0:Number = 0;
public var y0:Number = 0;
public var z0:Number = 0;
public var x1:Number = 0;
public var y1:Number = 0;
public var z1:Number = 0;
public var x2:Number = 0;
public var y2:Number = 0;
public var z2:Number = 0;
public var nx:Number = 0;
public var ny:Number = 0;
public var nz:Number = 0;
public function calcNormal():void
{
var ax0:Number; var ay0:Number; var az0:Number;
var bx0:Number; var by0:Number; var bz0:Number;
var mag:Number;
ax0 = x1 - x0; ay0 = y1 - y0; az0 = z1 - z0;
bx0 = x2 - x0; by0 = y2 - y0; bz0 = z2 - z0;
nx = (ay0 * bz0) - (az0 * by0);
ny = (az0 * bx0) - (ax0 * bz0);
nz = (ax0 * by0) - (ay0 * bx0);
mag = Math.sqrt(nx*nx + ny*ny +nz*nz);
if (mag == 0) {mag = 0.00001;}
mag = 1 / mag;
nx *= mag; ny *= mag; nz *= mag;
}//calcnormal
//returns squared distance to triangle
public function getDist(wx:Number, wy:Number, wz:Number, ret:xPnt):Number
{
var ax0:Number; var ay0:Number; var az0:Number;
var bx0:Number; var by0:Number; var bz0:Number;
var ax1:Number; var ay1:Number; var az1:Number;
var bx1:Number; var by1:Number; var bz1:Number;
var ax2:Number; var ay2:Number; var az2:Number;
var bx2:Number; var by2:Number; var bz2:Number;
var dz0:Number; var dz1:Number; var dz2:Number;
var rx:Number; var ry:Number; var rz:Number;
var kx:Number; var ky:Number; var kz:Number;
var wd:Number;
var d:Number;
//uses the same idea as the 2D version
// http://wonderfl.net/c/b27F
// the main difference is that because we are in 3D
// we cannot just look at the sign of Z
// we determine which side of the edge we are on
// by taking the result of the cross product
// and dot product it with the triangle normal
// ( so the downside of this approach
// is that you need calculate the triangle normal beforehand
// of course this can be precalculated so its not a problem for static meshes )
ax0 = x1 - x0;
ay0 = y1 - y0;
az0 = z1 - z0;
bx0 = wx - x0;
by0 = wy - y0;
bz0 = wz - z0;
kx = (ay0 * bz0) - (az0 * by0);
ky = (az0 * bx0) - (ax0 * bz0);
kz = (ax0 * by0) - (ay0 * bx0);
dz0 = nx*kx + ny*ky + nz*kz;
ax1 = x2 - x1;
ay1 = y2 - y1;
az1 = z2 - z1;
bx1 = wx - x1;
by1 = wy - y1;
bz1 = wz - z1;
kx = (ay1 * bz1) - (az1 * by1);
ky = (az1 * bx1) - (ax1 * bz1);
kz = (ax1 * by1) - (ay1 * bx1);
dz1 = nx*kx + ny*ky + nz*kz;
ax2 = x0 - x2;
ay2 = y0 - y2;
az2 = z0 - z2;
bx2 = wx - x2;
by2 = wy - y2;
bz2 = wz - z2;
kx = (ay2 * bz2) - (az2 * by2);
ky = (az2 * bx2) - (ax2 * bz2);
kz = (ax2 * by2) - (ay2 * bx2);
dz2 = nx*kx + ny*ky + nz*kz;
//+++ //inside -- closest to plane
if (dz0 >= 0 && dz1 >= 0 && dz2 >=0)
{
d = (wx-x0)*nx + (wy-y0)*ny + (wz-z0)*nz;
if (ret) {
//closest point is the tested point projected to the triangle's plane
ret.px = wx - ( d * nx);
ret.py = wy - ( d * ny);
ret.pz = wz - ( d * nz);
}//endif2
d *= d; //we need to return the squared distance to keep things simple
}
else //-+- //vertex
if (dz0 < 0 && dz1 >= 0 && dz2 < 0)
{
d = bx0*bx0 + by0*by0 + bz0*bz0;
if (ret) { ret.px = x0; ret.py = y0; ret.pz = z0; }
}
else //--+ //vertex
if (dz0 < 0 && dz1 < 0 && dz2 >= 0)
{
d = bx1*bx1 + by1*by1 + bz1*bz1;
if (ret) { ret.px = x1; ret.py = y1; ret.pz = z1; }
}
else //+-- //vertex
if (dz0 >= 0 && dz1 < 0 && dz2 < 0)
{
d = bx2*bx2 + by2*by2 + bz2*bz2;
if (ret) { ret.px = x2; ret.py = y2; ret.pz = z2; }
}
else //-++ //edge
if (dz0 < 0 && dz1 >= 0 && dz2 >= 0)
{
wd = ((ax0 * bx0) + (ay0 * by0) + (az0*bz0) ) / ((ax0 * ax0) + (ay0 * ay0) + (az0*az0));
if (wd < 0) { wd = 0;} if (wd > 1) { wd = 1; }
rx = x0 + (x1 - x0) * wd;
ry = y0 + (y1 - y0) * wd;
rz = z0 + (z1 - z0) * wd;
if (ret) { ret.px = rx; ret.py = ry; ret.pz = rz;}
rx = wx-rx; ry = wy-ry; rz = wz-rz;
d = rx*rx + ry*ry + rz*rz;
}
else //+-+ //edge
if (dz0 >= 0 && dz1 < 0 && dz2 >= 0)
{
wd = ((ax1 * bx1) + (ay1 * by1) + (az1*bz1) ) / ((ax1 * ax1) + (ay1 * ay1) + (az1*az1));
if (wd < 0) { wd = 0;} if (wd > 1) { wd = 1; }
rx = x1 + (x2 - x1) * wd;
ry = y1 + (y2 - y1) * wd;
rz = z1 + (z2 - z1) * wd;
if (ret) { ret.px = rx; ret.py = ry; ret.pz = rz;}
rx = wx-rx; ry = wy-ry; rz = wz-rz;
d = rx*rx + ry*ry + rz*rz;
}
else //++- //edge
if (dz0 >= 0 && dz1 >= 0 && dz2 < 0)
{
wd = ((ax2 * bx2) + (ay2 * by2) + (az2*bz2) ) / ((ax2 * ax2) + (ay2 * ay2) + (az2*az2));
if (wd < 0) { wd = 0;} if (wd > 1) { wd = 1; }
rx = x2 + (x0 - x2) * wd;
ry = y2 + (y0 - y2) * wd;
rz = z2 + (z0 - z2) * wd;
if (ret) { ret.px = rx; ret.py = ry; ret.pz = rz;}
rx = wx-rx; ry = wy-ry; rz = wz-rz;
d = rx*rx + ry*ry + rz*rz;
}//endif
return d;
}//isinside
}//xtris