|
|
e31ea0 |
|
|
|
b2ca59 |
#include <cassert></cassert>
|
|
|
b2ca59 |
|
|
|
e31ea0 |
#include "shape.h"
|
|
|
e31ea0 |
|
|
|
e31ea0 |
|
|
|
b2ca59 |
PointShape::PointShape(const Vector3 &pos, const Vector3 &dir) {
|
|
|
b2ca59 |
Vector3 vx = dir.perp().norm();
|
|
|
b2ca59 |
Vector3 vy = vx.cross(dir);
|
|
|
b2ca59 |
matrix = Matrix4(
|
|
|
b2ca59 |
vx.x , vx.y , vx.z , 0,
|
|
|
b2ca59 |
vy.x , vy.y , vy.z , 0,
|
|
|
b2ca59 |
dir.x, dir.y, dir.z, 0,
|
|
|
b2ca59 |
pos.x, pos.y, pos.z, 1 ).inv();
|
|
|
b2ca59 |
}
|
|
|
b2ca59 |
|
|
|
b2ca59 |
Real PointShape::base_distance_to_triangle(const Vector3 *v) {
|
|
|
b2ca59 |
Real dist = INFINITY;
|
|
|
b2ca59 |
|
|
|
b2ca59 |
Vector3 d[3] = {
|
|
|
b2ca59 |
v[1] - v[0],
|
|
|
b2ca59 |
v[2] - v[1],
|
|
|
b2ca59 |
v[0] - v[2] };
|
|
|
b2ca59 |
|
|
|
b2ca59 |
// collision with plane
|
|
|
b2ca59 |
Vector3 perp = d[1].cross(d[2]);
|
|
|
b2ca59 |
if (fabs(perp.z) > precision) {
|
|
|
b2ca59 |
// nearest plane touch point
|
|
|
b2ca59 |
Vector3 p(0, 0, perp*v[0]/perp.z);
|
|
|
b2ca59 |
|
|
|
b2ca59 |
// is touch point inside tringle
|
|
|
b2ca59 |
Real s = sign( perp.cross(d[0])*(p - v[0]), 0.1*precision );
|
|
|
b2ca59 |
if ( s
|
|
|
b2ca59 |
&& s == sign( perp.cross(d[1])*(p - v[1]), 0.1*precision )
|
|
|
b2ca59 |
&& s == sign( perp.cross(d[2])*(p - v[2]), 0.1*precision ) )
|
|
|
b2ca59 |
dist = std::min(dist, p.z);
|
|
|
b2ca59 |
}
|
|
|
b2ca59 |
|
|
|
b2ca59 |
return dist;
|
|
|
b2ca59 |
}
|
|
|
b2ca59 |
|
|
|
b2ca59 |
|
|
|
b2ca59 |
Real PointShape::distance_to_triangle(const Triangle &triangle) const {
|
|
|
b2ca59 |
Vector3 v[3] = {
|
|
|
b2ca59 |
matrix.transform( triangle.vertices[0] ),
|
|
|
b2ca59 |
matrix.transform( triangle.vertices[1] ),
|
|
|
b2ca59 |
matrix.transform( triangle.vertices[2] ) };
|
|
|
b2ca59 |
return base_distance_to_triangle(v);
|
|
|
b2ca59 |
}
|
|
|
b2ca59 |
|
|
|
b2ca59 |
|
|
|
b2ca59 |
|
|
|
b2ca59 |
CilinderShape::CilinderShape(const Vector3 &pos, const Vector3 &dir, Real radius) {
|
|
|
e31ea0 |
Vector3 vx = dir.perp().norm();
|
|
|
e31ea0 |
Vector3 vy = vx.cross(dir);
|
|
|
e31ea0 |
matrix = Matrix4(
|
|
|
e31ea0 |
vx.x*radius, vx.y*radius, vx.z*radius, 0,
|
|
|
e31ea0 |
vy.x*radius, vy.y*radius, vy.z*radius, 0,
|
|
|
e31ea0 |
dir.x , dir.y , dir.z , 0,
|
|
|
e31ea0 |
pos.x , pos.y , pos.z , 1 ).inv();
|
|
|
e31ea0 |
}
|
|
|
e31ea0 |
|
|
|
b2ca59 |
Real CilinderShape::base_distance_to_triangle(const Vector3 *v) {
|
|
|
e31ea0 |
Real dist = INFINITY;
|
|
|
e31ea0 |
|
|
|
b2ca59 |
Vector3 d[3];
|
|
|
e31ea0 |
Real A[3], B[3], C[3];
|
|
|
e31ea0 |
for(int i = 0; i < 3; ++i) {
|
|
|
e31ea0 |
C[i] = v[i].x*v[i].x + v[i].y*v[i].y - 1;
|
|
|
e31ea0 |
|
|
|
e31ea0 |
// collision with vertex
|
|
|
e31ea0 |
if (C[i] <= precision)
|
|
|
e31ea0 |
dist = std::min(dist, v[i].z);
|
|
|
e31ea0 |
}
|
|
|
e31ea0 |
|
|
|
e31ea0 |
for(int i = 0; i < 3; ++i) {
|
|
|
e31ea0 |
d[i] = v[(i+1)%3] - v[i];
|
|
|
e31ea0 |
A[i] = d[i].x*d[i].x + d[i].y*d[i].y;
|
|
|
e31ea0 |
B[i] = 2*(d[i].x*v[i].x + d[i].y*v[i].y);
|
|
|
e31ea0 |
|
|
|
e31ea0 |
// collision with edge
|
|
|
e31ea0 |
Real roots[2];
|
|
|
e31ea0 |
int count = solve(roots, C[i], B[i], A[i]);
|
|
|
e31ea0 |
for(int j = 0; j < count; ++j)
|
|
|
e31ea0 |
if (roots[j] >= -precision && roots[j] <= 1 + precision)
|
|
|
e31ea0 |
dist = std::min(dist, d[i].z*roots[j] + v[i].z);
|
|
|
e31ea0 |
}
|
|
|
e31ea0 |
|
|
|
e31ea0 |
// collision with plane
|
|
|
e31ea0 |
Vector3 perp = d[1].cross(d[2]);
|
|
|
e31ea0 |
if (perp.z < 0) perp = Vector3(-perp.x, -perp.y, -perp.z);
|
|
|
e31ea0 |
if (perp.z > precision) {
|
|
|
e31ea0 |
// nearest plane touch point
|
|
|
e31ea0 |
Vector3 p(0, 0, perp*v[0]/perp.z);
|
|
|
e31ea0 |
Real xy = sqrt(perp.x*perp.x + perp.y*perp.y);
|
|
|
e31ea0 |
if (xy > precision) {
|
|
|
e31ea0 |
Real dxy = 1/xy;
|
|
|
e31ea0 |
p.x = perp.x*dxy;
|
|
|
e31ea0 |
p.y = perp.y*dxy;
|
|
|
e31ea0 |
p.z -= xy/perp.z;
|
|
|
e31ea0 |
}
|
|
|
e31ea0 |
|
|
|
e31ea0 |
// is touch point inside tringle
|
|
|
e31ea0 |
Real s = sign( perp.cross(d[0])*(p - v[0]), 0.1*precision );
|
|
|
e31ea0 |
if ( s
|
|
|
e31ea0 |
&& s == sign( perp.cross(d[1])*(p - v[1]), 0.1*precision )
|
|
|
e31ea0 |
&& s == sign( perp.cross(d[2])*(p - v[2]), 0.1*precision ) )
|
|
|
e31ea0 |
dist = std::min(dist, p.z);
|
|
|
e31ea0 |
}
|
|
|
e31ea0 |
|
|
|
e31ea0 |
return dist;
|
|
|
e31ea0 |
}
|
|
|
b2ca59 |
|
|
|
b2ca59 |
|
|
|
b2ca59 |
Real CilinderShape::distance_to_triangle(const Triangle &triangle) const {
|
|
|
b2ca59 |
Vector3 v[3] = {
|
|
|
b2ca59 |
matrix.transform( triangle.vertices[0] ),
|
|
|
b2ca59 |
matrix.transform( triangle.vertices[1] ),
|
|
|
b2ca59 |
matrix.transform( triangle.vertices[2] ) };
|
|
|
b2ca59 |
return base_distance_to_triangle(v);
|
|
|
b2ca59 |
}
|
|
|
b2ca59 |
|
|
|
b2ca59 |
|
|
|
b2ca59 |
ConeShape::ConeShape(const Vector3 &pos, const Vector3 &dir, Real radius, Real height, Real cut_radius) {
|
|
|
b2ca59 |
Vector3 vx = dir.perp().norm();
|
|
|
b2ca59 |
Vector3 vy = vx.cross(dir);
|
|
|
b2ca59 |
matrix = Matrix4(
|
|
|
b2ca59 |
vx.x*radius, vx.y*radius, vx.z*radius, 0,
|
|
|
b2ca59 |
vy.x*radius, vy.y*radius, vy.z*radius, 0,
|
|
|
b2ca59 |
dir.x , dir.y , dir.z , 0,
|
|
|
b2ca59 |
pos.x , pos.y , pos.z , 1 ).inv();
|
|
|
b2ca59 |
this->cut_radius = fabs(radius) > precision ? fabs(cut_radius/radius) : 0;
|
|
|
b2ca59 |
this->height = fabs(height);
|
|
|
b2ca59 |
}
|
|
|
b2ca59 |
|
|
|
b2ca59 |
Real ConeShape::base_distance_to_triangle(const Vector3 *v, Real height, Real cut_radius) {
|
|
|
b2ca59 |
Real dist = INFINITY;
|
|
|
b2ca59 |
|
|
|
b2ca59 |
height = fabs(height);
|
|
|
b2ca59 |
cut_radius = fabs(cut_radius);
|
|
|
b2ca59 |
|
|
|
b2ca59 |
dist = std::min(dist, CilinderShape::base_distance_to_triangle(v) + height);
|
|
|
b2ca59 |
if (cut_radius <= precision)
|
|
|
b2ca59 |
dist = std::min(dist, PointShape::base_distance_to_triangle(v));
|
|
|
b2ca59 |
if (cut_radius >= 1 - precision || height <= precision)
|
|
|
b2ca59 |
return dist;
|
|
|
b2ca59 |
|
|
|
b2ca59 |
Real kz = (1 - cut_radius)/height;
|
|
|
b2ca59 |
Real dkz = 1/kz;
|
|
|
b2ca59 |
Real d_cut_radius = 1/cut_radius;
|
|
|
b2ca59 |
|
|
|
b2ca59 |
Vector3 v_cut[3] = {
|
|
|
b2ca59 |
Vector3(v[0].x*d_cut_radius, v[0].y*d_cut_radius, v[0].z),
|
|
|
b2ca59 |
Vector3(v[1].x*d_cut_radius, v[1].y*d_cut_radius, v[1].z),
|
|
|
b2ca59 |
Vector3(v[2].x*d_cut_radius, v[2].y*d_cut_radius, v[2].z) };
|
|
|
b2ca59 |
dist = std::min(dist, CilinderShape::base_distance_to_triangle(v_cut));
|
|
|
b2ca59 |
|
|
|
b2ca59 |
|
|
|
b2ca59 |
Real xy2[3];
|
|
|
b2ca59 |
for(int i = 0; i < 3; ++i) {
|
|
|
b2ca59 |
xy2[i] = v[i].x*v[i].x + v[i].y*v[i].y;
|
|
|
b2ca59 |
|
|
|
b2ca59 |
// collision with vertex
|
|
|
b2ca59 |
if (xy2[i] < 1 && xy2[i] >= cut_radius*cut_radius) {
|
|
|
b2ca59 |
Real dz = (sqrt(xy2[i]) - cut_radius)*dkz;
|
|
|
b2ca59 |
dist = std::min(dist, v[i].z + dz);
|
|
|
b2ca59 |
}
|
|
|
b2ca59 |
}
|
|
|
b2ca59 |
|
|
|
b2ca59 |
// collision with edge
|
|
|
b2ca59 |
Real kz2 = kz*kz;
|
|
|
b2ca59 |
Real cut_z = cut_radius*dkz;
|
|
|
b2ca59 |
for(int i = 0; i < 3; ++i) {
|
|
|
b2ca59 |
Vector3 d = v[(i+1)%3] - v[i];
|
|
|
b2ca59 |
|
|
|
b2ca59 |
Real A = d.x*d.x + d.y*d.y - d.z*d.z*kz2;
|
|
|
b2ca59 |
if (A > precision) {
|
|
|
b2ca59 |
// B = oB + kB*z
|
|
|
b2ca59 |
Real oB = 2*(d.x*v[i].x + d.y*v[i].y);
|
|
|
b2ca59 |
Real kB = -2*d.z*kz2;
|
|
|
b2ca59 |
|
|
|
b2ca59 |
// C = oC + kC*z*z
|
|
|
b2ca59 |
Real oC = xy2[i];
|
|
|
b2ca59 |
Real kC = -kz2;
|
|
|
b2ca59 |
|
|
|
b2ca59 |
// B*B - 4AC = 0
|
|
|
b2ca59 |
Real a = kB*kB - 4*A*kC;
|
|
|
b2ca59 |
Real b = 2*oB*kB;
|
|
|
b2ca59 |
Real c = oB*oB - 4*A*oC;
|
|
|
b2ca59 |
|
|
|
b2ca59 |
Real roots[2];
|
|
|
b2ca59 |
int count = solve(roots, c, b, a);
|
|
|
b2ca59 |
for(int j = 0; j < count; ++j) {
|
|
|
b2ca59 |
Real B = oB + kB*roots[j];
|
|
|
b2ca59 |
Real l = -0.5*B/A;
|
|
|
b2ca59 |
|
|
|
b2ca59 |
//Vector3 pp = d*l + Vector3(v[i].x, v[i].y, roots[j]);
|
|
|
b2ca59 |
//assert(fabs(pp.x*pp.x + pp.y*pp.y - kz2*pp.z*pp.z) < precision);
|
|
|
b2ca59 |
|
|
|
b2ca59 |
if (l >= -precision && l <= 1 + precision) {
|
|
|
b2ca59 |
Real z = d.z*l + roots[j];
|
|
|
b2ca59 |
if (z < 0) {
|
|
|
b2ca59 |
Vector3 p = d*l + v[i];
|
|
|
b2ca59 |
Real r2 = p.x*p.x + p.y*p.y;
|
|
|
b2ca59 |
if (r2 <= 1 + precision && r2 >= cut_radius*cut_radius)
|
|
|
b2ca59 |
dist = std::min(dist, p.z - cut_z - z);
|
|
|
b2ca59 |
}
|
|
|
b2ca59 |
}
|
|
|
b2ca59 |
}
|
|
|
b2ca59 |
}
|
|
|
b2ca59 |
}
|
|
|
b2ca59 |
|
|
|
b2ca59 |
return dist;
|
|
|
b2ca59 |
}
|
|
|
b2ca59 |
|
|
|
b2ca59 |
Real ConeShape::distance_to_triangle(const Triangle &triangle) const {
|
|
|
b2ca59 |
Vector3 v[3] = {
|
|
|
b2ca59 |
matrix.transform( triangle.vertices[0] ),
|
|
|
b2ca59 |
matrix.transform( triangle.vertices[1] ),
|
|
|
b2ca59 |
matrix.transform( triangle.vertices[2] ) };
|
|
|
b2ca59 |
return base_distance_to_triangle(v, height, cut_radius);
|
|
|
b2ca59 |
}
|
|
|
b2ca59 |
|