void matrixLookat(GLfloat M[4*4],
float eyex, float eyey, float eyez,
float centerx, float centery, float centerz,
float upx, float upy, float upz) {
GLfloat F[3];
F[0] = centerx - eyex;
F[1] = centery - eyey;
F[2] = centerz - eyez;
norm3(F);
GLfloat U[3];
U[0] = upx;
U[1] = upy;
U[2] = upz;
GLfloat S[3];
CROSS(F,U,S);
norm3(S);
CROSS(S,F,U);
GLfloat R[4*4];
matrixSet3x3(R,
S[0], S[1], S[2],
U[0], U[1], U[2],
-F[0], -F[1], -F[2]);
matrixCat(M, R);
matrixTranslate(M, -eyex, -eyey, -eyez);
}
double blob_segment_distance(double k,double *p1,double *p2,double *p,double q1,
double q2)
/*******************************************************************************
LAST MODIFIED : 30 August 1996
DESCRIPTION :
Calculates distance potential at a point p from a blob segment p1-p2 of charge
(q1,q2)
==============================================================================*/
{
double b[3],dist,r1,r2;
double return_code,v[3];
int i;
ENTER(blob_segment_distance);
/* default return value */
return_code=0.0;
/* checking arguments */
if (p1&&p2&&p)
{
for (i=0;i<3;i++)
{
b[i]=p[i]-p1[i];
v[i]=p[i]-p2[i];
}
r1=norm3(b);
r2=norm3(v);
if (0==r1*r2)
{
return_code=TEXTURE_LINE_INFINITY;
}
else
{
dist=(r1*r2)/(r1+r2);
return_code=k/(dist*dist)*(r2/(r1+r2)*q1+r1/(r1+r2)*q2);
}
}
else
{
display_message(ERROR_MESSAGE,
"blob_segment_distance. Invalid argument(s)");
return_code=0;
}
LEAVE;
return (return_code);
} /* blob_segment_distance */
double line_segment_distance(double k,double *p1,double *p2,double *p,double q1,
double q2)
/*******************************************************************************
LAST MODIFIED : 30 August 1996
DESCRIPTION :
Calculates distance potential at a point p from a line segment p1-p2 of charge
(q1,q2)
==============================================================================*/
{
double a = 0.0,a1,b[3],dist,r[3],return_code,v[3];
int i;
ENTER(line_segment_distance);
/* default return value */
return_code=0.0;
/* checking arguments */
if (p1&&p2&&p)
{
for (i=0;i<3;i++)
{
b[i]=p[i]-p1[i];
v[i]=p2[i]-p1[i];
}
if (0!=(a1=dot_product3(v,v)))
{
a=dot_product3(b,v)/a1;
}
if ((a>=0.0)&&(a<=1.0))
{
/* point does not extend further than line segment */
for(i=0;i<3;i++)
{
r[i]=b[i]-a*v[i];
}
if ((dist=norm3(r)) != 0)
{
return_code=k/(dist*dist)*(q1+a*(q2-q1));
}
else
{
return_code=TEXTURE_LINE_INFINITY;
}
}
else
{
return_code=0.0;
}
}
else
{
display_message(ERROR_MESSAGE,
"line_segment_distance. Invalid argument(s)");
return_code=0.0;
}
LEAVE;
return (return_code);
} /* line_segment_distance */
int direction3(double* dir, double* xyz)
{
double n = norm3(xyz);
dir[0] = xyz[0]/n;
dir[1] = xyz[1]/n;
dir[2] = xyz[2]/n;
return 0;
}
double qmTriangle(const double &xa, const double &ya, const double &za,
const double &xb, const double &yb, const double &zb,
const double &xc, const double &yc, const double &zc,
const qualityMeasure4Triangle &cr)
{
double quality;
switch(cr){
case QMTRI_RHO:
{
// quality = rho / R = 2 * inscribed radius / circumradius
double a [3] = {xc - xb, yc - yb, zc - zb};
double b [3] = {xa - xc, ya - yc, za - zc};
double c [3] = {xb - xa, yb - ya, zb - za};
norme(a);
norme(b);
norme(c);
double pva [3]; prodve(b, c, pva); const double sina = norm3(pva);
double pvb [3]; prodve(c, a, pvb); const double sinb = norm3(pvb);
double pvc [3]; prodve(a, b, pvc); const double sinc = norm3(pvc);
if (sina == 0.0 && sinb == 0.0 && sinc == 0.0) quality = 0.0;
else quality = 2 * (2 * sina * sinb * sinc / (sina + sinb + sinc));
}
break;
// condition number
case QMTRI_COND:
{
/*
double a [3] = {xc - xa, yc - ya, zc - za};
double b [3] = {xb - xa, yb - ya, zb - za};
double c [3] ; prodve(a, b, c); norme(c);
double A[3][3] = {{a[0] , b[0] , c[0]} ,
{a[1] , b[1] , c[1]} ,
{a[2] , b[2] , c[2]}};
*/
quality = -1;
}
break;
default:
Msg::Error("Unknown quality measure");
return 0.;
}
return quality;
}
void ObjectEdgeDetectorCPU::applyNormalCalculation() {
const int r = m_data->params.normalRange;
Vec fa1, fb1, n1, n01;
Channel32f filteredI = m_data->filteredImage[0];
for (int y = 0; y < m_data->h; y++) {
for (int x = 0; x < m_data->w; x++) {
int i = x + m_data->w * y;
if (y < r || y >= m_data->h - r || x < r
|| x >= m_data->w - r) {
m_data->normals[i].x = 0; //points out of range
m_data->normals[i].y = 0;
m_data->normals[i].z = 0;
} else {
//cross product normal determination
fa1[0] = (x + r) - (x - r);
fa1[1] = (y - r) - (y - r);
fa1[2] = filteredI(x + r, y - r)
- filteredI(x - r, y - r);
fb1[0] = (x) - (x - r);
fb1[1] = (y + r) - (y - r);
fb1[2] = filteredI(x, y + r)
- filteredI(x - r, y - r);
n1[0] = fa1[1] * fb1[2] - fa1[2] * fb1[1];
n1[1] = fa1[2] * fb1[0] - fa1[0] * fb1[2];
n1[2] = fa1[0] * fb1[1] - fa1[1] * fb1[0];
n01[0] = n1[0] / norm3(n1);
n01[1] = n1[1] / norm3(n1);
n01[2] = n1[2] / norm3(n1);
m_data->normals[i].x = n01[0];
m_data->normals[i].y = n01[1];
m_data->normals[i].z = n01[2];
m_data->normals[i].w = 1;
}
}
}
if (m_data->params.useNormalAveraging && !m_data->params.useGaussSmoothing) {
applyLinearNormalAveraging();
} else if (m_data->params.useNormalAveraging && m_data->params.useGaussSmoothing) {
applyGaussianNormalSmoothing();
}
}
void add_sphere_to_stencil(sphere *s, graphics_state *state){
f3 *eye = (f3*)state->projection_matrix + 3;
f3 diff = (f3){
s->center.x - eye->x,
s->center.y - eye->y,
s->center.z - eye->z};
int x0 = (int)state->width *(0.5f+((diff.x-RADIUS)/norm3(&diff))) - 1;
int x1 = (int)state->width *(0.5f+((diff.x+RADIUS)/norm3(&diff))) + 1;
int y0 = (int)state->height*(0.5f-((diff.y+RADIUS)/norm3(&diff))) - 1;
int y1 = (int)state->height*(0.5f-((diff.y-RADIUS)/norm3(&diff))) + 1;
if(x0 < 0) x0 = 0;
if(x1 >= state->width) x1 = state->width - 1;
if(y0 < 0) y0 = 0;
if(y1 >= state->height) y1 = state->height - 1;
int x,y;
for(y = y0; y <= y1; y++)
for(x = x0; x <= x1; x++)
state->stencil[x + y*state->width] = 1;
}
void Sphere::draw() {
glBegin(GL_TRIANGLES);
for (float y = 0; y < m_segmentsY; ++y) {
float lowerY = 0.5 - y / m_segmentsY;
float upperY = 0.5 - (y+1.0) / m_segmentsY;
float lowerR = transverseSliceRadiusSphere(0.5,lowerY);
float upperR = transverseSliceRadiusSphere(0.5,upperY);
for (int i = 0; i < m_segmentsX; ++i) {
float lowerXL = ithSliceXComponent(lowerR,i,m_segmentsX);
float upperXL = ithSliceXComponent(lowerR,i+1,m_segmentsX);
float lowerZL = ithSliceZComponent(lowerR,i,m_segmentsX);
float upperZL = ithSliceZComponent(lowerR,i+1,m_segmentsX);
float lowerXU = ithSliceXComponent(upperR,i,m_segmentsX);
float upperXU = ithSliceXComponent(upperR,i+1,m_segmentsX);
float lowerZU = ithSliceZComponent(upperR,i,m_segmentsX);
float upperZU = ithSliceZComponent(upperR,i+1,m_segmentsX);
Vector norm1(lowerXL,lowerY,lowerZL);
norm1.normalize();
glNormal3f(norm1[0],norm1[1],norm1[2]);
glVertex3f(lowerXL,lowerY,lowerZL);
Vector norm2(upperXL,lowerY,upperZL);
norm2.normalize();
glNormal3f(norm2[0],norm2[1],norm2[2]);
glVertex3f(upperXL,lowerY,upperZL);
Vector norm3(lowerXU,upperY,lowerZU);
norm3.normalize();
glNormal3f(norm3[0],norm3[1],norm3[2]);
glVertex3f(lowerXU,upperY,lowerZU);
Vector norm4(lowerXU,upperY,lowerZU);
norm4.normalize();
glNormal3f(norm4[0],norm4[1],norm4[2]);
glVertex3f(lowerXU,upperY,lowerZU);
Vector norm5(upperXL,lowerY,upperZL);
norm5.normalize();
glNormal3f(norm5[0],norm5[1],norm5[2]);
glVertex3f(upperXL,lowerY,upperZL);
Vector norm6(upperXU,upperY,upperZU);
norm6.normalize();
glNormal3f(norm6[0],norm6[1],norm6[2]);
glVertex3f(upperXU,upperY,upperZU);
}
}
glEnd();
}
double soft_object_distance(double WR,double *p1,double *p,double q1)
/*******************************************************************************
LAST MODIFIED : 4 March 1997
DESCRIPTION :
==============================================================================*/
{
double b[3], r1, w1;
double wr2, wr4, wr6, WR2, WR4, WR6;
double return_code;
int i;
ENTER(soft_object_distance);
WR2 = WR*WR;
WR4 = WR2*WR2;
WR6 = WR2*WR4;
/* default return value */
return_code=0.0;
/* checking arguments */
if (p1&&p)
{
for (i=0;i<3;i++)
{
b[i]=p[i]-p1[i];
}
r1=norm3(b);
/* wyvill function on distance */
if (r1 > WR)
{
return_code=0;
}
else
{
wr2= r1*r1;
wr4 = wr2*wr2;
wr6 = wr2*wr4;
w1 = q1*(1-4/9*wr6/WR6+17/9*wr4/WR4-22/9*wr2/WR2);
return_code=w1;
}
}
else
{
display_message(ERROR_MESSAGE,
"soft_object_distance. Invalid argument(s)");
return_code=0;
}
LEAVE;
return (return_code);
} /* soft_object_distance */
void tracer_bresenham(t_clist *param, t_clist *a, t_clist *b)
{
int dx;
int dy;
stock_param(param, a, b);
CONTENT(9) = set_color(a, b);
norm1(param, &dx, &dy);
if (dx > dy)
norm2(param, dx, dy);
else
norm3(param, dx, dy);
}
float dist_point_linesegment(const Vec4 &p,const Vec4 &lineStart,const Vec4 &lineEnd,Vec4 *nearestPoint){
Vec4 d1(p-lineStart);
Vec4 d2(lineEnd-lineStart);
Vec4 min_v(lineStart);
float t = sprod3(d2,d2);
if(t > std::numeric_limits<float>::min()){
t = sprod3(d1,d2)/t;
if(t > 1.0){
d1 = p - (min_v = lineEnd);
}else if(t > 0.f){
d1 = p - (min_v = lineStart + d2*t);
}
}
if(nearestPoint) *nearestPoint = min_v;
return norm3(d1);
}
bool RK4STEP_3D(double *gradientArray, int *gradientArraySize, double *startPoint, double *nextPoint, double stepSize) {
double k1[3], k2[3], k3[3], k4[3];
double tempPoint[3];
double tempnorm;
/*Calculate k1 */
interpgrad3d(k1, gradientArray, gradientArraySize, startPoint);
tempnorm=norm3(k1);
k1[0] = k1[0]*stepSize/tempnorm;
k1[1] = k1[1]*stepSize/tempnorm;
k1[2] = k1[2]*stepSize/tempnorm;
tempPoint[0]=startPoint[0] - k1[0]*0.5;
tempPoint[1]=startPoint[1] - k1[1]*0.5;
tempPoint[2]=startPoint[2] - k1[2]*0.5;
/*Check the if are still inside the domain */
if (!checkBounds3d(tempPoint, gradientArraySize)) return false;
/*Calculate k2 */
interpgrad3d(k2, gradientArray, gradientArraySize, tempPoint);
tempnorm=norm3(k2);
k2[0] = k2[0]*stepSize/tempnorm;
k2[1] = k2[1]*stepSize/tempnorm;
k2[2] = k2[2]*stepSize/tempnorm;
tempPoint[0]=startPoint[0] - k2[0]*0.5;
tempPoint[1]=startPoint[1] - k2[1]*0.5;
tempPoint[2]=startPoint[2] - k2[2]*0.5;
/*Check the if are still inside the domain */
if (!checkBounds3d(tempPoint, gradientArraySize)) return false;
/*Calculate k3 */
interpgrad3d(k3, gradientArray, gradientArraySize, tempPoint);
tempnorm=norm3(k3);
k3[0] = k3[0]*stepSize/tempnorm;
k3[1] = k3[1]*stepSize/tempnorm;
k3[2] = k3[2]*stepSize/tempnorm;
tempPoint[0]=startPoint[0] - k3[0];
tempPoint[1]=startPoint[1] - k3[1];
tempPoint[2]=startPoint[2] - k3[2];
/*Check the if are still inside the domain */
if (!checkBounds3d(tempPoint, gradientArraySize)) return false;
/*Calculate k4 */
interpgrad3d(k4, gradientArray, gradientArraySize, tempPoint);
tempnorm=norm3(k4);
k4[0] = k4[0]*stepSize/tempnorm;
k4[1] = k4[1]*stepSize/tempnorm;
k4[2] = k4[2]*stepSize/tempnorm;
/*Calculate final point */
nextPoint[0] = startPoint[0] - (k1[0] + k2[0]*2.0 + k3[0]*2.0 + k4[0])/6.0;
nextPoint[1] = startPoint[1] - (k1[1] + k2[1]*2.0 + k3[1]*2.0 + k4[1])/6.0;
nextPoint[2] = startPoint[2] - (k1[2] + k2[2]*2.0 + k3[2]*2.0 + k4[2])/6.0;
/*Check the if are still inside the domain */
if (!checkBounds3d(nextPoint, gradientArraySize)) return false;
return true;
}
double curve_segment_distance(double k,double *p1,double *p2,double *p3,
double *p4,double *p,double q1,double q2)
/*******************************************************************************
LAST MODIFIED : 30 August 1996
DESCRIPTION :
Calculates distance potential at a point p from a curve segment defined by
p1-p2, slope p3,p4 with charge(q1,q2) the distance Q(t)-p is minimized by
translating to the origin and solving the minimum sum Q(t)*Q(t) =>
0 = sum Q'(t).Q(t)
==============================================================================*/
{
double at,bt,ct,dist,dt,min_dist,min_t = 0.0,pt1[3],pt2[3],pt3[3],pt4[3],t;
/* intermediate calculations */
double a[3],b[3],c[3],d[3],e[3],f[3],g[3],r[3];
/* coefficients of Q'(t).Q(t) */
double return_code;
fcomplex coeff[6];
/* roots */
fcomplex roots[6];
int real_roots[6];
int i,j,min,n_real_roots;
static double val;
ENTER(curve_segment_distance);
/* default return value */
return_code=0.0;
/* checking arguments */
if (p1&&p2&&p3&&p4&&p)
{
for (i=0;i<6;i++)
{
coeff[i].r=0;
coeff[i].i=0;
roots[i].r=0;
roots[i].i=0;
}
for (i=0;i<3;i++)
{
/* translate to origin */
pt1[i]=p1[i]-p[i];
pt2[i]=p2[i]-p[i];
pt3[i]=p3[i]-p[i] ;
pt4[i]=p4[i]-p[i];
/* Q(t) */
a[i]=-pt1[i]+3*pt2[i]-3*pt3[i]+pt4[i];
b[i]=3*pt1[i]-6*pt2[i]+3*pt3[i];
c[i]=-3*pt1[i]+3*pt2[i];
d[i]=pt1[i];
/* Q'(t) */
e[i]=-3*pt1[i]+9*pt2[i]-9*pt3[i]+3*pt4[i];
f[i]=6*pt1[i]-12*pt2[i]+6*pt3[i];
g[i]=-3*pt1[i]+3*pt2[i];
}
/* calculate coefficients of minimized polynomial */
for (i=0;i<3;i++)
{
coeff[0].r += (GLfloat)d[i]*g[i];
coeff[1].r += (GLfloat)c[i]*g[i]+d[i]*f[i];
coeff[2].r += (GLfloat)b[i]*g[i]+c[i]*f[i]+d[i]*e[i];
coeff[3].r += (GLfloat)a[i]*g[i]+b[i]*f[i]+c[i]*e[i];
coeff[4].r += (GLfloat)a[i]*f[i]+b[i]*e[i];
coeff[5].r += (GLfloat)a[i]*e[i];
}
/* find roots of polynomial */
/* the real root is the t parameter value of the curve pt nearest p */
zroots(coeff,5,roots,1);
min=1;
n_real_roots=0;
/* choose closest to real root if no real root exists*/
for (i=1;i<6;i++)
{
/* printf"Root[%d] = (%lf + %lfi)\n",i,roots[i].r,roots[i].i);*/
if ((roots[i].i*roots[i].i<roots[min].i*roots[min].i)||(roots[i].i == 0))
{
if (roots[i].i == 0)
{
real_roots[n_real_roots]=i;
n_real_roots++;
}
min=i;
}
}
if (n_real_roots == 0)
{
real_roots[n_real_roots]=min;
n_real_roots=1;
}
/* we have our best selection of real roots - now must find */
/* which one is the minimum */
min_dist=0.;
for (i=0;i<n_real_roots;i++)
{
t=(double)roots[real_roots[i]].r;
at=(1-t)*(1-t)*(1-t);
bt=3.0*t*(1-t)*(1-t);
ct=3.0*t*t*(1-t);
dt=t*t*t;
for (j=0;j<3;j++)
{
r[j]=at*pt1[j]+bt*pt2[j]+ct*pt3[j]+dt*pt4[j];
}
dist=norm3(r);
if (dist<min_dist||i==0)
{
min_dist=dist;
min_t=t;
}
}
if ((min_t>=0)&&(min_t<=1.0))
{
if (0!=min_dist)
{
val=k/(min_dist*min_dist)*(q1+min_t*(q2-q1));
return_code=val;
}
else
{
return_code=TEXTURE_LINE_INFINITY;
}
}
else
{
return_code=0.0;
}
}
else
{
display_message(ERROR_MESSAGE,
"curve_segment_distance. Invalid argument(s)");
return_code=0.0;
}
LEAVE;
return (return_code);
} /* curve_segment_distance */
int get_normal_theta_map(double* theta_map, double* centered_point_map,
int* step_left, int* step_right,
int height, int width)
{
int h,w, pw, nw;
double * xyz_hw, * xyz_phw, * xyz_nhw;
double * pt_p, * pt_n, *pt_c;
double theta_p, theta_n, theta_c;
double dist_p, dist_n, dist_c;
int k;
xyz_hw = (double *) malloc(3*sizeof(double));
xyz_phw = (double *) malloc(3*sizeof(double));
xyz_nhw = (double *) malloc(3*sizeof(double));
pt_p = (double *) malloc(2*sizeof(double));
pt_n = (double *) malloc(2*sizeof(double));
pt_c = (double *) malloc(2*sizeof(double));
for (h = 0; h < height; h++)
{
for (w = 0; w < width; w++)
{
pw = step_left[height*width + h*width + w];
nw = step_right[height*width + h*width + w];
theta_c = -1000;
for (k = 0; k < 3; k++)
{
xyz_hw[k] = centered_point_map[k*height*width+h*width+w];
xyz_phw[k] = centered_point_map[k*height*width+h*width+pw];
xyz_nhw[k] = centered_point_map[k*height*width+h*width+nw];
}
if (norm3(xyz_hw) > 0.01)
{
direction_between_vectors_phi_theta(pt_p, xyz_phw, xyz_hw);
direction_between_vectors_phi_theta(pt_n, xyz_hw, xyz_nhw);
direction_between_vectors_phi_theta(pt_c, xyz_phw, xyz_nhw);
dist_p = distance3(xyz_phw,xyz_hw);
dist_n = distance3(xyz_hw,xyz_nhw);
dist_c = distance3(xyz_phw,xyz_nhw);
theta_p = pt_p[1]-PI/2;
theta_n = pt_n[1]-PI/2;
if (norm3(xyz_nhw) > 0.01 && norm3(xyz_phw) && dist_c > 0.01)
{
theta_c = pt_c[1]-PI/2;
}
else if (norm3(xyz_nhw) > 0.01 && dist_n > 0.01)
{
theta_c = theta_n;
}
else if (norm3(xyz_phw) > 0.01 && dist_p > 0.01)
{
theta_c = theta_p;
}
if (theta_c <= -PI && theta_c != -1000)
{
theta_c = theta_c + 2*PI;
}
}
theta_map[h*width+w] = theta_c;
}
}
free(xyz_hw);
free(xyz_phw);
free(xyz_nhw);
free(pt_p);
free(pt_n);
}
void Spine::update_mesh()
{
if (!dirty_mesh)
return;
int N = n * p * q;
int M = m;
while ((N + 1) * (M + 1) >= 65536)
N--;
dirty_mesh = false;
vec3 points[(N + 1) * (M + 1)];
vec3 norms[(N + 1) * (M + 1)];
vec4 params[(N + 1) * (M + 1)];
struct sv2
{
unsigned short a, b;
};
sv2 quad_indices[N * (M + 1)];
for (int i = 0; i <= N; ++i)
{
Turns t_(i, N);
Radians t = t_;
vec3 Ct = C(t);
vec3 Bt = B(t);
norm3(Bt);
vec3 Nt = this->N(t);
norm3(Nt);
for (int j = 0; j <= M; ++j)
{
Turns u_(j, M);
Radians u = u_;
params[i * (M + 1) + j] = {t_.value() * q, u_.value(), 0, 1};
norms[i * (M + 1) + j] = s * cos_(u) * Bt + r * sin_(u) * Nt;
norm3(norms[i * (M + 1) + j]);
points[i * (M + 1) + j] = Ct + r * cos_(u) * Bt + s * sin_(u) * Nt;
if (i == N)
continue;
quad_indices[i * (M + 1) + j] = {
(unsigned short)(i * (M + 1) + j),
(unsigned short)((i + 1) * (M + 1) + j),
};
}
}
char filename[256];
sprintf(filename, "data/spiral-%.10e,%.10e,%.10e,%.10e-%d,%d,%d,%d.mesh",
a, b, r, s,
p, q, N, M);
FILE *fp = fopen(filename, "w");
fprintf(fp, "textures:\n");
fprintf(fp, " ambient: data/earthlights1k.bmp\n");
fprintf(fp, " diffuse: data/earthmap1k.bmp\n");
fprintf(fp, " specular: data/earthspec1k.bmp\n");
fprintf(fp, "\nvertices:\n");
for (int i = 0; i < (N + 1) * (M + 1); ++i)
{
auto p = vec4(points[i], 1.0f);
auto t = params[i];
auto n = vec4(norms[i], 1.0f);
fprintf(fp, " - position: [%.10e, %.10e, %.10e, %.10e]\n", p.x, p.y, p.z, p.w);
fprintf(fp, " texture: [%.10e, %.10e, %.10e, %.10e]\n", t.x * this->q, t.y, t.z, t.w);
fprintf(fp, " normal: [%.10e, %.10e, %.10e, %.10e]\n", n.x, n.y, n.z, n.w);
}
fprintf(fp, "\nfaces:\n");
for (int j = 1; j < N * (M + 1); ++j)
{
ivec3 f1 =
{
quad_indices[j - 1].a,
quad_indices[j - 1].b,
quad_indices[j].a,
};
ivec3 f2 =
{
quad_indices[j - 1].b,
quad_indices[j].b,
quad_indices[j].a,
};
fprintf(fp, " - [%d, %d, %d]\n", f1.x, f1.y, f1.z);
fprintf(fp, " - [%d, %d, %d]\n", f2.x, f2.y, f2.z);
}
fclose(fp);
glBindBuffer(GL_ARRAY_BUFFER, mesh_points);
glBufferData(GL_ARRAY_BUFFER,
sizeof(points), points,
GL_STATIC_DRAW);
glBindBuffer(GL_ARRAY_BUFFER, mesh_norms);
glBufferData(GL_ARRAY_BUFFER,
sizeof(norms), norms,
GL_STATIC_DRAW);
glBindBuffer(GL_ARRAY_BUFFER, mesh_params);
glBufferData(GL_ARRAY_BUFFER,
sizeof(params), params,
GL_STATIC_DRAW);
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, mesh_indices);
glBufferData(GL_ELEMENT_ARRAY_BUFFER,
sizeof(quad_indices), quad_indices,
GL_STATIC_DRAW);
}
void timestep(graphics_state *state){
int h = state->frame;
/* eye, moves with time */
*((f3*)state->projection_matrix + 3) = (f3) {
0, //sinf(h / 30.0f),
0,
-4}; //-cosf(h / 30.0f)};
/* so does the light */
int i;
for(i = 0; i < state->light_count; i++){
light *light = state->lights + i;
light->location = (f4){
sinf(h / 50.0f + i)*cosf(h / 100.0f + i),
sinf(h/100.0f + i),
cosf(h / 50.0f + i)*cosf(h / 100.0f + i),
0.2f};
}
/* so do the objs */
for(i = 0; i < state->object_count; i++){
sphere *s = state->object_list + i;
s->center.x += s->velocity.x;
s->center.y += s->velocity.y;
s->center.z += s->velocity.z;
/* bounce off the bounding box */
if(s->center.x < state->bounds.top.x)
s->velocity.x = absf(s->velocity.x);
if(s->center.y < state->bounds.top.y)
s->velocity.y = absf(s->velocity.y);
if(s->center.z < state->bounds.top.z)
s->velocity.z = absf(s->velocity.z);
if(s->center.x > state->bounds.bottom.x)
s->velocity.x = -absf(s->velocity.x);
if(s->center.y > state->bounds.bottom.y)
s->velocity.y = -absf(s->velocity.y);
if(s->center.z > state->bounds.bottom.z)
s->velocity.z = -absf(s->velocity.z);
/* bounce off each other */
int j;
for(j = i+1; j < state->object_count; j++){
sphere *s2 = state->object_list + j;
f3 diff = (f3){
s->center.x - s2->center.x,
s->center.y - s2->center.y,
s->center.z - s2->center.z};
float dist = norm3(&diff);
if(dist < s->radius + s2->radius){
normalize3(&diff);
float dot = dot3(&diff, &s->velocity);
if(dot > 0)
/* if they're already flying apart,
* don't bounce them back towards each other */
continue;
float dot2 = dot3(&diff, &s2->velocity);
s->velocity.x -= diff.x*(dot - dot2);
s->velocity.y -= diff.y*(dot - dot2);
s->velocity.z -= diff.z*(dot - dot2);
s2->velocity.x -= diff.x*(dot2 - dot);
s2->velocity.y -= diff.y*(dot2 - dot);
s2->velocity.z -= diff.z*(dot2 - dot);
}
}
}
}
float trace(const ray *ray, const graphics_state *state, f3 *light, int max_depth){
*light = (f3){0,0,0};
f3 intersection, normal;
float t = intersect(ray, state, &intersection, &normal);
if(t < 0)
return -1;
/* shade; no shadows for now */
f3 specular = (f3){0.4f, 0.4f, 0.4f};
f3 diffuse = (f3){1,1,0.2f};
int j;
for(j = 0; j < state->light_count; j++){
f4 light_location = state->lights[j].location;
f3 light_direction = (f3){
light_location.x - intersection.x*light_location.w,
light_location.y - intersection.y*light_location.w,
light_location.z - intersection.z*light_location.w};
/* shadows -- these are slow.
struct ray light_ray;
light_ray.start = intersection;
light_ray.direction = light_direction;
f3 tmp_intersect, tmp_normal;
if(intersect(&light_ray, state, &tmp_intersect, &tmp_normal) > 0)
continue;*/
float cos_incident = dot3(&normal, &light_direction) / norm3(&light_direction);
if(cos_incident < 0)
cos_incident = 0;
cos_incident += 0.04f;
light->x += diffuse.x*cos_incident*state->lights[j].color.x;
light->y += diffuse.y*cos_incident*state->lights[j].color.y;
light->z += diffuse.z*cos_incident*state->lights[j].color.z;
}
/* recurse to get specular reflection */
float bounce = dot3(&ray->direction, &normal);
struct ray newRay;
newRay.direction = (f3){
ray->direction.x - 2*bounce*normal.x,
ray->direction.y - 2*bounce*normal.y,
ray->direction.z - 2*bounce*normal.z};
f3 newLight = (f3){0,0,0};
if(max_depth > 0){
newRay.start = intersection;
trace(&newRay, state, &newLight, max_depth-1);
}
/* special case: specular highlights */
for(j = 0; j < state->light_count; j++){
f4 light_location = state->lights[j].location;
f3 light_direction = (f3){
light_location.x - intersection.x*light_location.w,
light_location.y - intersection.y*light_location.w,
light_location.z - intersection.z*light_location.w};
float light_cos = dot3(&light_direction, &newRay.direction);
float thresh = 0.85;
if(light_cos > thresh){
float highlight = (light_cos - thresh)/(1-thresh);
highlight *= highlight * highlight * 3;
newLight.x += highlight*state->lights[j].color.x;
newLight.y += highlight*state->lights[j].color.y;
newLight.z += highlight*state->lights[j].color.z;
}
}
light->x += specular.x*newLight.x;
light->y += specular.y*newLight.y;
light->z += specular.z*newLight.z;
return t;
}
int get_normal_phi_map(double* phi_map, double* centered_point_map,
int* step_up, int* step_down,
int height, int width)
{
int h,w, ph, nh;
double * xyz_hw, * xyz_phw, * xyz_nhw;
double phi_p, phi_n, phi_c;
double dia_p, dia_c, dia_n;
double dz_p, dz_n, dz_c, dd_p, dd_n, dd_c;
double rad_p,rad_n,rad_c;
double dist_p, dist_n, dist_c;
int k;
xyz_hw = (double *) malloc(3*sizeof(double));
xyz_phw = (double *) malloc(3*sizeof(double));
xyz_nhw = (double *) malloc(3*sizeof(double));
for (w = 0; w < width; w++)
{
for (h = 0; h < height; h++)
{
ph = step_up[h*width + w];
nh = step_down[h*width + h];
phi_c = -1000;
for (k = 0; k < 3; k++)
{
xyz_hw[k] = centered_point_map[k*height*width+h*width+w];
xyz_phw[k] = centered_point_map[k*height*width+ph*width+w];
xyz_nhw[k] = centered_point_map[k*height*width+nh*width+w];
}
if (norm3(xyz_hw) > 0.01)
{
dist_p = distance3(xyz_phw,xyz_hw);
dist_n = distance3(xyz_hw,xyz_nhw);
dist_c = distance3(xyz_phw,xyz_nhw);
dia_p = sqrt(pow(xyz_phw[0],2) + pow(xyz_phw[1],2));
dia_c = sqrt(pow(xyz_hw[0],2) + pow(xyz_hw[1],2));
dia_n = sqrt(pow(xyz_nhw[0],2) + pow(xyz_nhw[1],2));
dd_p = dia_p - dia_c;
dd_n = dia_c - dia_n;
dd_c = dia_p - dia_n;
dz_p = xyz_phw[2]-xyz_hw[2];
dz_n = xyz_hw[2]-xyz_nhw[2];
dz_c = xyz_phw[2]-xyz_nhw[2];
rad_p = sqrt(pow(dd_p,2)+pow(dz_p,2));
rad_n = sqrt(pow(dd_n,2)+pow(dz_n,2));
rad_c = sqrt(pow(dd_c,2)+pow(dz_c,2));
phi_p = -asin(dd_p/rad_p);
phi_n = -asin(dd_n/rad_n);
if (norm3(xyz_nhw) > 0.01 && norm3(xyz_phw) && h > 0 && nh < height && dist_c > 0.01 && rad_c > 0)
{
phi_c = -asin(dd_c/rad_c);
}
else if (norm3(xyz_nhw) > 0.01 && nh < height && dist_n > 0.01 && rad_n > 0)
{
phi_c = -phi_n;
}
else if (norm3(xyz_phw) > 0.01 && h > 0 && dist_p > 0.01 && rad_p > 0)
{
phi_c = -phi_p;
}
}
phi_map[h*width+w] = phi_c;
}
}
free(xyz_hw);
free(xyz_phw);
free(xyz_nhw);
}
static int assemble3d(const int num, const int exportAxis, std::string &axisstr,
std::string &plotstr, double *eulerAngles, int *viewport,
double *proj, double *model, int ypix, int xpix)
{
if (exportAxis) {
axisstr.append("\tenlargelimits=false, %% tight axis, use xmin=<val>, ");
axisstr.append("xmax=<val> for custom bounding box\n");
axisstr.append("\tgrid=both,\n\tminor tick num=1,\n");
std::string xlab, ylab, zlab;
xlab = CTX::instance()->axesLabel[0];
ylab = CTX::instance()->axesLabel[1];
zlab = CTX::instance()->axesLabel[2];
if (xlab.empty())
xlab = "x";
if (ylab.empty())
ylab = "y";
if (zlab.empty())
zlab = "z";
axisstr.append("\txlabel={" + xlab + "}, %%\n\tylabel={" +
ylab + "},\n\tzlabel={" + zlab + "},\n");
axisstr.append("\tzlabel style={rotate=-90},\n"); // bug?
}
else {
if(opt_general_orthographic(0, GMSH_GET, 0) == 0 ) {
Msg::Warning("Axes are not orthogonal, but because you do not want "
"any axes, I'll continue with a 2d picture.");
return assemble2d(num, 0, axisstr, plotstr, eulerAngles);
}
if (CTX::instance()->camera) {
Msg::Warning("Camera output not supported, but since you do not want "
"any axes, I'll continue with a 2d picture.");
return assemble2d(num, 0, axisstr, plotstr, eulerAngles);
}
axisstr.append("\thide axis,\n");
}
if(opt_general_orthographic(0, GMSH_GET, 0) == 0 && exportAxis) {
Msg::Warning("Cannot produce output if axes are not orthogonal.");
Msg::Error("Please switch to orthographic projection mode "
"('Alt + o') and retry if you want to output axes.");
return 1;
}
double axPts[8][3];
getMinMaxOfAxis(num, axPts);
double factor=1.;
// requires the pixel coordinates of the axis ends (actually any four
// points with all different x/y/z <pixX, pixY> would suffice)
double axViewPt[8][3];
std::vector<int> acceptableAnchors;
std::vector<int> masked;
bool reorder = false;
double minlen = 0.;
std::string suffix;
for (unsigned int j=0; j < 8; j++) {
// project the 8 axis end points to pixel coordinates,
// accept if they are in the screen range.
gluProject(axPts[j][0], axPts[j][1], axPts[j][2], model, proj,
viewport, &axViewPt[j][0], &axViewPt[j][1], &axViewPt[j][2]);
// printf("x=%f, y=%f, z=%f\n", axPts[j][0], axPts[j][1], axPts[j][2]);
// printf("xprn=%f, yprn=%f, zprn=%f\n",
// axViewPt[j][0], axViewPt[j][1], axViewPt[j][2]);
if ((int)(axViewPt[j][0]+0.5) <= xpix &&
(int)(axViewPt[j][1]+0.5) <= ypix) {
acceptableAnchors.push_back(j);
}
else
masked.push_back(j);
if (j>0) {
// respecting TeXs range limts (1e-4 relative precision)
minlen = norm3(axPts[j]);
//fprintf(stderr,"j=%d, vec length %f:\n", j, minlen);
if(minlen < 1e-5) {
factor=1e6;
suffix.assign("/$\\mu$m");
}
else if(minlen < 0.01) {
factor=1e3;
suffix.assign("/mm");
}
else if(minlen > 1e6) {
factor=1e-6;
suffix.assign("/Mm");
}
else if(minlen > 1000) {
factor=1e-3;
suffix.assign("/Km");
}
}
if (j == 1 && acceptableAnchors.size() == 2) {
// precaution: if the first two coordinates are accepted, a
// division by zero can occur in pgfplots
// furthermore, four points with equal x=xmin leads to
// singular system in pgfplots
reorder=true;
acceptableAnchors.pop_back();
}
}
if (reorder) acceptableAnchors.push_back(1);
if (acceptableAnchors.size() < 4) {
Msg::Error("Unable to calculate anchors for pgf output. "
"Make sure the entire scene is visible or adjust "
"the axes min/max values to fit inside your screen.");
return 2;
}
if (factor != 1) {
char tmp[265];
sprintf(tmp, "The pgf output has been rescaled in order to please "
"the TeX number precision/range. Rescaling your results by "
"a factor %g", factor);
Msg::Warning(tmp);
// replace three labels
if (exportAxis) {
// xlabel={x<>}, %%
// ylabel={y<>},
// zlabel={z<>},
// zlabel style={rotate=-90},
std::size_t foundrot = axisstr.rfind("},");
std::size_t foundz = axisstr.rfind("},", foundrot-1);
if (foundz!=std::string::npos)
axisstr.insert(foundz,suffix);
else
return 4;
std::size_t foundy = axisstr.rfind("},",foundz);
if (foundy!=std::string::npos)
axisstr.insert(foundy,suffix);
else
return 4;
std::size_t foundx = axisstr.rfind("},", foundy);
if (foundx!=std::string::npos)
axisstr.insert(foundx,suffix);
else
return 4;
}
}
char tmp[265];
plotstr.assign("\t \\addplot3[surf] graphics[debug=false,%%=visual,\n");
plotstr.append("\t points={%%\n");
unsigned int j=0;
for (std::vector<int>::iterator it = acceptableAnchors.begin();
it != acceptableAnchors.end(); ++it, j++) {
sprintf(tmp,"\t (%f,%f,%f)", factor*axPts[*it][0],
factor*axPts[*it][1], factor*axPts[*it][2]);
plotstr.append(tmp);
if (j > 3) {
plotstr.append("%%");
}
// ypix-y syntax for easier debugging w/ e.g. gimp pixel
sprintf(tmp," => (%d, %d-%d)\n", (int)(axViewPt[*it][0]+0.5),
ypix,ypix-(int)(axViewPt[*it][1]+0.5));
plotstr.append(tmp);
}
for (std::vector<int>::iterator it = masked.begin();
it != masked.end(); ++it) {
sprintf(tmp,"\t (%f,%f,%f)", factor*axPts[*it][0],
factor*axPts[*it][1], factor*axPts[*it][2]);
plotstr.append(tmp);
plotstr.append(" %% out of pixel range, discarded\n");
}
plotstr.append("\t }]\n");
return 0;
}
static int assemble2d(const int num, const int exportAxis, std::string &axisstr,
std::string &plotstr, double *eulerAngles)
{
double axPts[8][3];
double factor=1.;
double xmin, xmax, ymin, ymax;
axisstr.append("\taxis equal image, %% use png aspect ratio\n");
if (exportAxis) {
getMinMaxOfAxis(num, axPts);
std::string xlab, ylab, zlab;
xlab = CTX::instance()->axesLabel[0];
ylab = CTX::instance()->axesLabel[1];
zlab = CTX::instance()->axesLabel[2];
if (xlab.empty())
xlab = "x";
if (ylab.empty())
ylab = "y";
if (zlab.empty())
zlab = "z";
fprintf(stderr,"Euler two dim: 0:%f, 1:%f, 2:%f\n",
eulerAngles[0], eulerAngles[1], eulerAngles[2]);
int r0 = (int) (eulerAngles[0]+0.5);
int r1 = (int) (eulerAngles[1]+0.5);
int r2 = (int) (eulerAngles[2]+0.5);
if (r0 % 90 != 0 || r1 % 90 != 0 || r2 % 90 !=0) {
fprintf(stderr,"Euler two dim: 0:%d, 1:%d, 2:%d\n", r0, r1, r2);
Msg::Error("Please select a plane view (X, Y, Z)");
return 1;
}
if (r0 % 180 == 0 && r1 % 360 == 0 && r2 % 180 == 0) {
// xy
xmin=axPts[0][0]; xmax=axPts[4][0]; ymin=axPts[0][1]; ymax=axPts[2][1];
if (r2 == 180)
axisstr.append("\tx dir=reverse,\n");
if ((r2 == 180 && abs(r0) < 1) || (r0 == 180 && abs(r2) < 1))
axisstr.append("\ty dir=reverse,\n");
axisstr.append("\txlabel={" + xlab + "},\n");
axisstr.append("\tylabel={" + ylab + "},\n");
}
else if (r0 % 180 == 0 && r1 % 360 ==0 && (r2 == 90 || r2 == 270)) {
// yx
xmin=axPts[0][1]; xmax=axPts[2][1]; ymin=axPts[0][0]; ymax=axPts[4][0];
if (r2 == 90)
axisstr.append("\tx dir=reverse,\n");
if (r2 == 270 || (r2 == 90 && r0 == 180))
axisstr.append("\ty dir=reverse,\n");
axisstr.append("\txlabel={" + ylab + "},\n");
axisstr.append("\tylabel={" + xlab + "},\n");
}
else if ((r0 == 90 || r0 == 270) && r1 % 360 == 0 &&
(r2 == 90 || r2 == 270)) {
xmin=axPts[0][1]; xmax=axPts[2][1]; ymin=axPts[0][2]; ymax=axPts[1][2];
if(r2 == 90)
axisstr.append("\tx dir=reverse,\n");
if(r0 == 90)
axisstr.append("\ty dir=reverse,\n");
// yz
axisstr.append("\txlabel={" + ylab + "},\n");
axisstr.append("\tylabel={" + zlab + "},\n");
}
else if (r0 % 360 == 0 && (r1 == 90 || r1 == 270) && r2 % 180 == 0) {
// zy
xmin=axPts[0][2]; xmax=axPts[1][2]; ymin=axPts[0][1]; ymax=axPts[2][1];
if (r1 == 270)
axisstr.append("\tx dir=reverse,\n");
if (r2 == 180)
axisstr.append("\ty dir=reverse,\n");
axisstr.append("\txlabel={" + zlab + "},\n");
axisstr.append("\tylabel={" + ylab + "},\n");
}
else if ((r0 == 90 || r0 == 270) && r1 % 360 == 0 && r2 % 180 == 0) {
// xz
xmin=axPts[0][0]; xmax=axPts[4][0]; ymin=axPts[0][2]; ymax=axPts[1][2];
if (r2 == 180) // x dir=reverse
axisstr.append("\tx dir=reverse,\n");
if (r0 == 90)
axisstr.append("\ty dir=reverse,\n");
axisstr.append("\txlabel={" + xlab + "},\n");
axisstr.append("\tylabel={" + zlab + "},\n");
}
else if (r0 % 360 == 0 && (r1 == 90 || r1 == 270) &&
(r2 == 90 || r2 == 270)) {
if (r1 == 270)
axisstr.append("\tx dir=reverse,\n");
if (r2 == 270)
axisstr.append("\ty dir=reverse,\n");
// zx
xmin=axPts[0][2]; xmax=axPts[1][2]; ymin=axPts[0][0]; ymax=axPts[4][0];
axisstr.append("\txlabel={" + zlab + "},\n");
axisstr.append("\tylabel={" + xlab + "},\n");
}
else {
Msg::Error("Cannot infer orientation from Euler angles...");
// this should not happen
//error=true;
return 2;
}
if (fabs(xmax - xmin) < 1e-8 ||
fabs(ymax - ymin) < 1e-8) {
Msg::Error("I inferred x (%f) or y (%f) dimension to be zero. Cannot produce.",
fabs(xmax - xmin), fabs(ymax - ymin));
return 3;
}
double diagonal[3] = {xmax-xmin, ymax-ymin, 0};
double minlen = norm3(diagonal);
std::string suffix;
if(minlen < 1e-5) {
factor=1e6;
suffix.assign(" / $\\mu$m");
}
else if(minlen < 0.01) {
factor=1e3;
suffix.assign(" / mm");
}
else if(minlen > 1e6) {
factor=1e-6;
suffix.assign(" / Mm");
}
else if(minlen > 1000) {
factor=1e-3;
suffix.assign(" / Km");
}
if (factor != 1) {
char tmp[265];
sprintf(tmp, "The pgf output has been rescaled in order to please "
"the TeX number precision/range. Rescaling your results by "
"a factor %g", factor);
Msg::Warning(tmp);
// sprintf(tmp, "$\\times 10^{%d}$},",(int)(log10(factor)+0.5));
// std::string repl = tmp;
// replace two labels
std::size_t foundy = axisstr.rfind("},");
if (foundy!=std::string::npos)
axisstr.insert(foundy,suffix);
else
return 4;
std::size_t foundx = axisstr.rfind("},", foundy);
if (foundx!=std::string::npos)
axisstr.insert(foundx,suffix);
else
return 4;
}
// axis options
axisstr.append("\tenlargelimits=false, %% tight axis, use xmin=<val>, ");
axisstr.append("xmax=<val> for custom bounding box\n");
axisstr.append("\taxis on top,\n\tscale only axis,\n");
}
else {
// no axis
xmin=0, xmax=1, ymin=0, ymax=1;
axisstr.append("\thide axis,\n");
}
char tmp[265];
sprintf(tmp,"\t \\addplot graphics[xmin=%f, xmax=%f, ymin=%f, ymax=%f]\n",
xmin*factor, xmax*factor, ymin*factor, ymax*factor);
plotstr.assign(tmp);
return 0;
}
float CurvatureFeatureExtractor::linePointDistance(std::pair<Vec,Vec> line, Vec point){
float d = norm3(cross(point-line.first,point-line.second))/norm3(line.second-line.first);
return d;
}
float dist_point_triangle(const Vec4 &p,const Vec4 &a,const Vec4 &b,const Vec4 &c,Vec4 *nearestPoint){
Vec4 ab = b - a;
Vec4 ac = c - a;
Vec4 n = cross(ab,ac); // not normalized !
float d = sqrnorm3(n);
// Check if the triangle is degenerated -> measure dist to line segments
if(fabs(d) < std::numeric_limits<float>::min()){
Vec4 q, qq;
float d, dd(std::numeric_limits<float>::max());
dd = dist_point_linesegment(p, a, b, &qq);
d = dist_point_linesegment(p, b, c, &q);
if(d < dd) { dd = d; qq = q; }
d = dist_point_linesegment(p, c, a, &q);
if(d < dd) { dd = d; qq = q; }
if(nearestPoint) *nearestPoint = qq;
return dd;
}
float invD = 1.0 / d;
Vec4 v1v2 = c; v1v2 -= b;
Vec4 v0p = p; v0p -= a;
Vec4 t = cross(v0p,n);
float aa= sprod3(t,ac) * -invD;
float bb = sprod3(t,ab) * invD;
float s01, s02, s12;
// Calculate the distance to an edge or a corner vertex
if(aa < 0){
s02 = sprod3(ac,v0p) / sqrnorm3(ac);
if(s02 < 0.0){
s01 = sprod3(ab,v0p) / sqrnorm3(ab);
if(s01 <= 0.0){
v0p = a;
}else if(s01 >= 1.0){
v0p = b;
}else{
(v0p = a) += (ab *= s01);
}
}else if(s02 > 1.0){
s12 = sprod3(v1v2,( p - b )) / sqrnorm3(v1v2);
if(s12 >= 1.0){
v0p = c;
}else if(s12 <= 0.0){
v0p = b;
}else{
(v0p = b) += (v1v2 *= s12);
}
}else{
(v0p = a) += (ac *= s02);
}
}else if(bb < 0.0){// Calculate the distance to an edge or a corner vertex
s01 = sprod3(ab,v0p) / sqrnorm3(ab);
if(s01 < 0.0){
s02 = sprod3(ac,v0p) / sqrnorm3(ac);
if(s02 <= 0.0){
v0p = a;
}else if(s02 >= 1.0){
v0p = c;
}else{
(v0p = a) += (ac *= s02);
}
}else if(s01 > 1.0){
s12 = sprod3(v1v2,( p - b )) / sqrnorm3(v1v2);
if(s12 >= 1.0){
v0p = c;
}else if(s12 <= 0.0){
v0p = b;
}else{
(v0p = b) += (v1v2 *= s12);
}
}else{
(v0p = a) += (ab *= s01);
}
}else if(aa+bb > 1.0){ // Calculate the distance to an edge or a corner vertex
s12 = sprod3(v1v2,( p - b )) / sqrnorm3(v1v2);
if(s12 >= 1.0){
s02 = sprod3(ac,v0p) / sqrnorm3(ac);
if(s02 <= 0.0){
v0p = a;
}else if(s02 >= 1.0){
v0p = c;
}else{
(v0p = a) += (ac *= s02);
}
}else if(s12 <= 0.0){
s01 = sprod3(ab,v0p) / sqrnorm3(ab);
if(s01 <= 0.0){
v0p = a;
}else if(s01 >= 1.0){
v0p = b;
}else {
(v0p = a) += (ab *= s01);
}
}else{
(v0p = b) += (v1v2 *= s12);
}
}else{// Calculate the distance to an interior point of the triangle
n *= (sprod3(n,v0p) * invD);
(v0p = p) -= n;
}
if(nearestPoint) *nearestPoint = v0p;
v0p -= p;
return norm3(v0p);
}
