// compute the vector VEC between a 3D point P0 and a line (
// passing through two 3D points P and S)
double vec_pt_to_line3D(double *P,double *S,double *P0,double *VEC)
{
double diff10[3];
double diff21[3];
double dot_prod_diff10_diff21;
double t,norm_diff21;
double VT[3];
double diffVT0[3];
double norm_diffVT0;
VEC_DIFF(diff10,P,P0);
VEC_DIFF(diff21,S,P);
VEC_LENGTH(norm_diff21,diff21);
VEC_DOT_PRODUCT(dot_prod_diff10_diff21,diff10,diff21);
t=-dot_prod_diff10_diff21 / ( norm_diff21*norm_diff21 );
VEC_COPY(VT,P)
VEC_ACCUM(VT,t,diff21);
VEC_DIFF(diffVT0,VT,P0);
for(int t=0;t<3;t++)
{
VEC[t] = P0[t];
VEC[t+3] = VT[t];
}
VEC_LENGTH(norm_diffVT0,diffVT0);
return norm_diffVT0;
}
// distance between a 3D point P and a line (defined by its
// normalized direction vector V and passing through a 3D point S)
double dist_line_point3D(double *V,double *S,double *P)
{
double cross_prod[3];
double sm[3];
double norm;
sm[0] = P[0] - S[0];
sm[1] = P[1] - S[1];
sm[2] = P[2] - S[2];
VEC_CROSS_PRODUCT(cross_prod,V,sm);
VEC_LENGTH(norm,cross_prod);
return norm;
}
// Solve a NLP problem
//
// Function input:
//
// double (*fhg) (Vector x, Vector h, Vector g):
// input: x: n-Vector (unknown variables)
// output: h: m-Vector (equality constraints)
// output: g: p-Vector (inequality constraints)
// void (*Dfhg)(Vector x, Vector df, Matrix dh, Matrix dg): (optional, can be set to NULL):
// input: x: n-Vector (unknown variables)
// output: Df: n-Vector (function gradient)
// output: Dh: m-by-n Matrix (Jacobian equality constraints)
// output: Dg: p-by-n Matrix (Jacobian inequality constraints)
// x: n-Vector
// input: solution estimate, output: solution to the NLP
// lambda: m-Vector: Lagrange multipliers associated with the equality constraints
// mu: p-Vector: Lagrange multipliers associated with the inequality constraints
// options:
// option.method = L1SQPmethod | LinfSQPmethod | Hybridmethod
// option.tolerance = 1.0e-6;
// option.maximumIterations = 1000;
// option.displayData = NO;
//
// Function output:
//
// stats: NLPStatistics data structure
// stats.f: function value
// stats.normc: norm of the active constraints
// stats.normT: norm of the KKT conditions
// stats.numberOfIterations;
// stats.numberOfFunctionCalls;
// stats.numberOfGradientCalls;
// stats.numberOfQPSolves;
// stats.numberOfRefinements;
// stats.numberOfSOC;
// stats.exitCode;
NLPStatistics
NLPsolve(double (*fhg) (Vector x, Vector h, Vector g),
void (*Dfhg) (Vector x, Vector df, Matrix dh, Matrix dg),
Vector x, Vector lambda, Vector mu,
NLPOptions options) {
NLPStatistics stats;
int n = 0, m = 0, p = 0;
stats.exitCode = 0;
stats.f = 0;
stats.normc = 0;
stats.normT = 0;
stats.numberOfIterations = 0;
stats.numberOfFunctionCalls = 0;
stats.numberOfGradientCalls = 0;
stats.numberOfQPSolves = 0;
stats.numberOfRefinements = 0;
stats.numberOfSOC = 0;
if (x == NULL) {
RuntimeWarning("NLPsolve: no initial solution estimate given");
return stats;
} else {
n = VEC_LENGTH(x);
}
if (lambda != NULL) {
m = VEC_LENGTH(lambda);
}
if (mu != NULL) {
p = VEC_LENGTH(mu);
}
NLP nlp = NLPNew();
nlp->n = n;
nlp->m = m;
nlp->p = p;
nlp->fhg = fhg;
nlp->Dfhg = Dfhg;
stats.n = n;
stats.m = m;
stats.p = p;
if (options.method == L1SQPmethod) {
L1SQP sqp0 = L1SQPNew(nlp);
sqp0->tolerance = options.tolerance;
sqp0->maximumIterations = options.maximumIterations;
sqp0->displayData = options.displayData;
stats.exitCode = L1SQPSolve(sqp0, x, lambda, mu);
stats.f = sqp0->f;
stats.normc = sqp0->normc;
stats.normT = sqp0->normT;
stats.numberOfIterations = sqp0->numberOfIterations;
stats.numberOfFunctionCalls = sqp0->numberOfFunctionCalls;
stats.numberOfGradientCalls = sqp0->numberOfGradientCalls;
stats.numberOfQPSolves = sqp0->numberOfQPSolves;
stats.numberOfRefinements = sqp0->numberOfRefinements;
stats.numberOfSOC = sqp0->numberOfSOC;
sprintf(stats.methodName, "%s", "L1SQPmethod");
L1SQPDelete(sqp0);
} else if (options.method == LinfSQPmethod) {
LinfSQP sqp1 = LinfSQPNew(nlp);
sqp1->tolerance = options.tolerance;
sqp1->maximumIterations = options.maximumIterations;
sqp1->displayData = options.displayData;
stats.exitCode = LinfSQPSolve(sqp1, x, lambda, mu);
stats.f = sqp1->f;
stats.normc = sqp1->normc;
stats.normT = sqp1->normT;
stats.numberOfIterations = sqp1->numberOfIterations;
stats.numberOfFunctionCalls = sqp1->numberOfFunctionCalls;
stats.numberOfGradientCalls = sqp1->numberOfGradientCalls;
stats.numberOfQPSolves = sqp1->numberOfQPSolves;
stats.numberOfRefinements = sqp1->numberOfRefinements;
stats.numberOfSOC = sqp1->numberOfSOC;
sprintf(stats.methodName, "%s", "LinfSQPmethod");
LinfSQPDelete(sqp1);
} /* else if (options.method == HybridSQPmethod) {
L1SQP sqp2 = L1SQPNew(nlp);
sqp2->tolerance = options.tolerance;
sqp2->maximumIterations = options.maximumIterations;
sqp2->displayData = options.displayData;
stats.exitCode = L1SQPSolveX(sqp2, x, lambda, mu);
stats.f = sqp2->f;
stats.normc = sqp2->normc;
stats.normT = sqp2->normT;
stats.numberOfIterations = sqp2->numberOfIterations;
stats.numberOfFunctionCalls = sqp2->numberOfFunctionCalls;
stats.numberOfGradientCalls = sqp2->numberOfGradientCalls;
stats.numberOfQPSolves = sqp2->numberOfQPSolves;
stats.numberOfRefinements = sqp2->numberOfRefinements;
stats.numberOfSOC = sqp2->numberOfSOC;
L1SQPDelete(sqp2);
} else if (options.method == LagNewtmethod) {
LagNewt sqp3 = LagNewtNew(nlp);
sqp3->tolerance = options.tolerance;
sqp3->maximumIterations = options.maximumIterations;
sqp3->displayData = options.displayData;
stats.exitCode = LagNewtSolve(sqp3, x, lambda, mu);
stats.f = sqp3->f;
stats.normc = sqp3->normc;
stats.normT = sqp3->normT;
stats.numberOfIterations = sqp3->numberOfIterations;
stats.numberOfFunctionCalls = sqp3->numberOfFunctionCalls;
stats.numberOfGradientCalls = sqp3->numberOfGradientCalls;
stats.numberOfQPSolves = sqp3->numberOfQPSolves;
stats.numberOfRefinements = sqp3->numberOfRefinements;
//stats.numberOfSOC = sqp3->numberOfSOC;
LagNewtDelete(sqp3);
}*/ else {
RuntimeWarning("NLPsolve: invalid solution method specified");
}
NLPDelete(nlp);
return stats;
}
/*!
\pre tv_plane and tu_plane must be set
\post
distances[2] is set with the distance
closest_point_u, closest_point_v, edge_edge_dir are set too
\return
- 0: faces are paralele
- 1: face U casts face V
- 2: face V casts face U
- 3: nearest edges
*/
SIMD_FORCE_INLINE GUINT cross_line_intersection_test()
{
// Compute direction of intersection line
edge_edge_dir = tu_plane.cross(tv_plane);
GREAL Dlen;
VEC_LENGTH(edge_edge_dir,Dlen);
if(Dlen<0.0001)
{
return 0; //faces near paralele
}
edge_edge_dir*= 1/Dlen;//normalize
// Compute interval for triangle 1
GUINT tu_e0,tu_e1;//edge indices
GREAL tu_scale_e0,tu_scale_e1;//edge scale
if(!compute_intervals(du[0],du[1],du[2],
du0du1,du0du2,tu_scale_e0,tu_scale_e1,tu_e0,tu_e1)) return 0;
// Compute interval for triangle 2
GUINT tv_e0,tv_e1;//edge indices
GREAL tv_scale_e0,tv_scale_e1;//edge scale
if(!compute_intervals(dv[0],dv[1],dv[2],
dv0dv1,dv0dv2,tv_scale_e0,tv_scale_e1,tv_e0,tv_e1)) return 0;
//proyected vertices
btVector3 up_e0 = tu_vertices[tu_e0].lerp(tu_vertices[(tu_e0+1)%3],tu_scale_e0);
btVector3 up_e1 = tu_vertices[tu_e1].lerp(tu_vertices[(tu_e1+1)%3],tu_scale_e1);
btVector3 vp_e0 = tv_vertices[tv_e0].lerp(tv_vertices[(tv_e0+1)%3],tv_scale_e0);
btVector3 vp_e1 = tv_vertices[tv_e1].lerp(tv_vertices[(tv_e1+1)%3],tv_scale_e1);
//proyected intervals
GREAL isect_u[] = {up_e0.dot(edge_edge_dir),up_e1.dot(edge_edge_dir)};
GREAL isect_v[] = {vp_e0.dot(edge_edge_dir),vp_e1.dot(edge_edge_dir)};
sort_isect(isect_u[0],isect_u[1],tu_e0,tu_e1,up_e0,up_e1);
sort_isect(isect_v[0],isect_v[1],tv_e0,tv_e1,vp_e0,vp_e1);
const GREAL midpoint_u = 0.5f*(isect_u[0]+isect_u[1]); // midpoint
const GREAL midpoint_v = 0.5f*(isect_v[0]+isect_v[1]); // midpoint
if(midpoint_u<midpoint_v)
{
if(isect_u[1]>=isect_v[1]) // face U casts face V
{
return 1;
}
else if(isect_v[0]<=isect_u[0]) // face V casts face U
{
return 2;
}
// closest points
closest_point_u = up_e1;
closest_point_v = vp_e0;
// calc edges and separation
if(isect_u[1]+ MIN_EDGE_EDGE_DIS<isect_v[0]) //calc distance between two lines instead
{
SEGMENT_COLLISION(
tu_vertices[tu_e1],tu_vertices[(tu_e1+1)%3],
tv_vertices[tv_e0],tv_vertices[(tv_e0+1)%3],
closest_point_u,
closest_point_v);
edge_edge_dir = closest_point_u-closest_point_v;
VEC_LENGTH(edge_edge_dir,distances[2]);
edge_edge_dir *= 1.0f/distances[2];// normalize
}
else
{
distances[2] = isect_v[0]-isect_u[1];//distance negative
//edge_edge_dir *= -1.0f; //normal pointing from V to U
}
}
else
{
if(isect_v[1]>=isect_u[1]) // face V casts face U
{
return 2;
}
else if(isect_u[0]<=isect_v[0]) // face U casts face V
{
return 1;
}
// closest points
closest_point_u = up_e0;
closest_point_v = vp_e1;
// calc edges and separation
if(isect_v[1]+MIN_EDGE_EDGE_DIS<isect_u[0]) //calc distance between two lines instead
{
SEGMENT_COLLISION(
tu_vertices[tu_e0],tu_vertices[(tu_e0+1)%3],
tv_vertices[tv_e1],tv_vertices[(tv_e1+1)%3],
closest_point_u,
closest_point_v);
edge_edge_dir = closest_point_u-closest_point_v;
VEC_LENGTH(edge_edge_dir,distances[2]);
edge_edge_dir *= 1.0f/distances[2];// normalize
}
else
{
distances[2] = isect_u[0]-isect_v[1];//distance negative
//edge_edge_dir *= -1.0f; //normal pointing from V to U
}
}
return 3;
}
/*
* The uviewdirection subroutine computes and returns a 4x4 rotation
* matrix that puts the negative z axis along the direction v21 and
* puts the y axis along the up vector.
*
* Note that this code is fairly tolerant of "weird" paramters.
* It normalizes when necessary, it does nothing when vectors are of
* zero length, or are co-linear. This code shouldn't croak, no matter
* what the user sends in as arguments.
*/
void uview_direction (gleDouble m[4][4], /* returned */
gleDouble v21[3], /* input */
gleDouble up[3]) /* input */
{
gleDouble amat[4][4];
gleDouble bmat[4][4];
gleDouble cmat[4][4];
gleDouble v_hat_21[3];
gleDouble v_xy[3];
gleDouble sine, cosine;
gleDouble len;
gleDouble up_proj[3];
gleDouble tmp[3];
/* find the unit vector that points in the v21 direction */
VEC_COPY (v_hat_21, v21);
VEC_LENGTH (len, v_hat_21);
if (len != 0.0) {
len = 1.0 / len;
VEC_SCALE (v_hat_21, len, v_hat_21);
/* rotate z in the xz-plane until same latitude */
sine = sqrt ( 1.0 - v_hat_21[2] * v_hat_21[2]);
ROTY_CS (amat, (-v_hat_21[2]), (-sine));
} else {
/* error condition: zero length vecotr passed in -- do nothing */
IDENTIFY_MATRIX_4X4 (amat);
}
/* project v21 onto the xy plane */
v_xy[0] = v21[0];
v_xy[1] = v21[1];
v_xy[2] = 0.0;
VEC_LENGTH (len, v_xy);
/* rotate in the x-y plane until v21 lies on z axis ---
* but of course, if its already there, do nothing */
if (len != 0.0) {
/* want xy projection to be unit vector, so that sines/cosines pop out */
len = 1.0 / len;
VEC_SCALE (v_xy, len, v_xy);
/* rotate the projection of v21 in the xy-plane over to the x axis */
ROTZ_CS (bmat, v_xy[0], v_xy[1]);
/* concatenate these together */
MATRIX_PRODUCT_4X4 (cmat, amat, bmat);
} else {
/* no-op -- vector is already in correct position */
COPY_MATRIX_4X4 (cmat, amat);
}
/* up vector really should be perpendicular to the x-form direction --
* Use up a couple of cycles, and make sure it is,
* just in case the user blew it.
*/
VEC_PERP (up_proj, up, v_hat_21);
VEC_LENGTH (len, up_proj);
if (len != 0.0) {
/* normalize the vector */
len = 1.0/len;
VEC_SCALE (up_proj, len, up_proj);
/* compare the up-vector to the y-axis to get the cosine of the angle */
tmp [0] = cmat [1][0];
tmp [1] = cmat [1][1];
tmp [2] = cmat [1][2];
VEC_DOT_PRODUCT (cosine, tmp, up_proj);
/* compare the up-vector to the x-axis to get the sine of the angle */
tmp [0] = cmat [0][0];
tmp [1] = cmat [0][1];
tmp [2] = cmat [0][2];
VEC_DOT_PRODUCT (sine, tmp, up_proj);
/* rotate to align the up vector with the y-axis */
ROTZ_CS (amat, cosine, -sine);
/* This xform, although computed last, acts first */
MATRIX_PRODUCT_4X4 (m, amat, cmat);
} else {
/* error condition: up vector is indeterminate (zero length)
* -- do nothing */
COPY_MATRIX_4X4 (m, cmat);
}
}
int gim_triangle_sphere_collision(
GIM_TRIANGLE_DATA *tri,
vec3f center, GREAL radius,
GIM_TRIANGLE_CONTACT_DATA * contact_data)
{
contact_data->m_point_count = 0;
//Find Face plane distance
GREAL dis = DISTANCE_PLANE_POINT(tri->m_planes.m_planes[0],center);
if(dis>radius) return 0; //out
if(dis<-radius) return 0;//Out of triangle
contact_data->m_penetration_depth = dis;
//Find the most edge
GUINT32 most_edge = 4;//no edge
GREAL max_dis = 0.0f;
dis = DISTANCE_PLANE_POINT(tri->m_planes.m_planes[1],center);
if(dis>radius) return 0;//Out of triangle
if(dis>0.0f)
{
max_dis = dis;
most_edge = 0;
}
dis = DISTANCE_PLANE_POINT(tri->m_planes.m_planes[2],center);
if(dis>radius) return 0;//Out of triangle
if(dis>max_dis)// && dis>0.0f)
{
max_dis = dis;
most_edge = 1;
}
dis = DISTANCE_PLANE_POINT(tri->m_planes.m_planes[3],center);
if(dis>radius) return 0;//Out of triangle
if(dis>max_dis)// && dis>0.0f)
{
max_dis = dis;
most_edge = 2;
}
if(most_edge == 4) //Box is into triangle
{
//contact_data->m_penetration_depth = dis is set above
//Find Face plane point
VEC_COPY(contact_data->m_separating_normal,tri->m_planes.m_planes[0]);
//Find point projection on plane
if(contact_data->m_penetration_depth>=0.0f)
{
VEC_SCALE(contact_data->m_points[0],-radius,contact_data->m_separating_normal);
}
else
{
VEC_SCALE(contact_data->m_points[0],radius,contact_data->m_separating_normal);
}
contact_data->m_penetration_depth = radius - contact_data->m_penetration_depth;
VEC_SUM(contact_data->m_points[0],contact_data->m_points[0],center);
//Scale normal for pointing to triangle
VEC_SCALE(contact_data->m_separating_normal,-1.0f,contact_data->m_separating_normal);
contact_data->m_point_count = 1;
return 1;
}
//find the edge
vec3f e1,e2;
VEC_COPY(e1,tri->m_vertices[most_edge]);
VEC_COPY(e2,tri->m_vertices[(most_edge+1)%3]);
CLOSEST_POINT_ON_SEGMENT(contact_data->m_points[0],center,e1,e2);
//find distance
VEC_DIFF(e1,center,contact_data->m_points[0]);
VEC_LENGTH(e1,dis);
if(dis>radius) return 0;
contact_data->m_penetration_depth = radius - dis;
if(IS_ZERO(dis))
{
VEC_COPY(contact_data->m_separating_normal,tri->m_planes.m_planes[most_edge+1]);
VEC_SCALE(contact_data->m_points[0],-radius,contact_data->m_separating_normal);
VEC_SUM(contact_data->m_points[0],contact_data->m_points[0],center);
}
else
{
VEC_SCALE(contact_data->m_separating_normal,1.0f/dis,e1);
VEC_SCALE(contact_data->m_points[0],-radius,contact_data->m_separating_normal);
VEC_SUM(contact_data->m_points[0],contact_data->m_points[0],center);
}
//Scale normal for pointing to triangle
VEC_SCALE(contact_data->m_separating_normal,-1.0f,contact_data->m_separating_normal);
contact_data->m_point_count = 1;
return 1;
}
void gleLathe (int ncp, /* number of contour points */
gleDouble contour[][2], /* 2D contour */
gleDouble cont_normal[][2], /* 2D contour normals */
gleDouble up[3], /* up vector for contour */
gleDouble startRadius,
gleDouble drdTheta, /* change in radius per revolution */
gleDouble startZ,
gleDouble dzdTheta, /* change in Z per revolution */
gleDouble startXform[2][3],
gleDouble dXformdTheta[2][3], /* tangent change xform per revln */
gleDouble startTheta, /* start angle, in degrees */
gleDouble sweepTheta) /* sweep angle, in degrees */
{
gleDouble localup[3];
gleDouble len;
gleDouble trans[2];
gleDouble start[2][3], delt[2][3];
/* Because the spiral always starts on the axis, and proceeds in the
* positive y direction, we can see that valid up-vectors must lie
* in the x-z plane. Therefore, we make sure we have a valid up
* vector by projecting it onto the x-z plane, and normalizing. */
if (up[1] != 0.0) {
localup[0] = up[0];
localup[1] = 0.0;
localup[2] = up[2];
VEC_LENGTH (len, localup);
if (len != 0.0) {
len = 1.0/len;
localup[0] *= len;
localup[2] *= len;
VEC_SCALE (localup, len, localup);
} else {
/* invalid up vector was passed in */
localup[0] = 0.0;
localup[2] = 1.0;
}
} else {
VEC_COPY (localup, up);
}
/* the dzdtheta derivative and the drdtheta derivative form a vector
* in the x-z plane. dzdtheta is the z component, and drdtheta is
* the x component. We need to convert this vector into the local
* coordinate system defined by the up vector. We do this by
* applying a 2D rotation matrix.
*/
trans[0] = localup[2] * drdTheta - localup[0] * dzdTheta;
trans[1] = localup[0] * drdTheta + localup[2] * dzdTheta;
/* now, add this translation vector into the affine xform */
if (startXform != NULL) {
if (dXformdTheta != NULL) {
COPY_MATRIX_2X3 (delt, dXformdTheta);
delt[0][2] += trans[0];
delt[1][2] += trans[1];
} else {
/*Hmm- the transforms don't exist */
delt[0][0] = 0.0;
delt[0][1] = 0.0;
delt[0][2] = trans[0];
delt[1][0] = 0.0;
delt[1][1] = 0.0;
delt[1][2] = trans[1];
}
gleSpiral (ncp, contour, cont_normal, up,
startRadius, 0.0, startZ, 0.0,
startXform, delt,
startTheta, sweepTheta);
} else {
/* Hmm- the transforms don't exist */
start[0][0] = 1.0;
start[0][1] = 0.0;
start[0][2] = 0.0;
start[1][0] = 0.0;
start[1][1] = 1.0;
start[1][2] = 0.0;
delt[0][0] = 0.0;
delt[0][1] = 0.0;
delt[0][2] = trans[0];
delt[1][0] = 0.0;
delt[1][1] = 0.0;
delt[1][2] = trans[1];
gleSpiral (ncp, contour, cont_normal, up,
startRadius, 0.0, startZ, 0.0,
start, delt,
startTheta, sweepTheta);
}
}
