*/
public ROVector2f getNearestPoint(ROVector2f p) {
// TODO: this can be done with a kind of binary search
float r = Float.MAX_VALUE;
float l;
Vector2f v;
int m = -1;
for ( int i = 0; i < vertices.length; i++ ) {
v = new Vector2f(vertices[i]);
v.sub(p);
l = v.x * v.x + v.y * v.y;
if ( l < r ) {
r = l;
m = i;
}
}
// the closest point could be on one of the closest point's edges
// this happens when the angle between v[m-1]-v[m] and p-v[m] is
// smaller than 90 degrees, same for v[m+1]-v[m]
int length = vertices.length;
Vector2f pm = new Vector2f(p);
pm.sub(vertices[m]);
Vector2f l1 = new Vector2f(vertices[(m-1+length)%length]);
l1.sub(vertices[m]);
Vector2f l2 = new Vector2f(vertices[(m+1)%length]);
l2.sub(vertices[m]);
Vector2f normal;
if ( pm.dot(l1) > 0 ) {
normal = MathUtil.getNormal(vertices[(m-1+length)%length], vertices[m]);
} else if ( pm.dot(l2) > 0 ) {
normal = MathUtil.getNormal(vertices[m], vertices[(m+1)%length]);
} else {
return vertices[m];
}
normal.scale(-pm.dot(normal));
normal.add(p);
return normal;
}