cv::Mat mylib::BinomialKernel(int size, int nDataType)
{
cv::Mat rst(size,size,nDataType);
if (nDataType == CV_32F)
{
vector<float> kernel(2,0.5);
vector<float> temp(2,0.5);
for (int n=0;n<size-2;n++)
{
temp = conv(temp,kernel,0);
}
rst = mylib::VecToMat(crossproduct(temp,temp),nDataType);
return rst;
}
else
{
vector<double> kernel(2,0.5);
vector<double> temp(2,0.5);
for (int n=0;n<size-2;n++)
{
temp = conv(temp,kernel,0);
}
rst = mylib::VecToMat(crossproduct(temp,temp),nDataType);
//mylib::DisplayMat(rst,"binomial kernal");
return rst;
}
}
// r.start = &worldPix point
// r.end = &direction vector
double RTTriangle::intersection(const ray &r)
{
// t is the distance with an intersected plane!
float t = (-1)*( dotproduct((*r.start - p0_), n_) / dotproduct(*r.end, n_));
// x is the point on the plane where hit
point x = *r.start + t*(*r.end);
// // cull back facing triangles
// if ( dotproduct(*r.start, n_) <= INTERSECTION_TOLERANCE )
// {
// return 0.0f;
// }
// first test if triangle is behind camera or if normal and direction vector orthogonal (t = 0)
if ( t < INTERSECTION_TOLERANCE )
{
return 0.0f;
}
// testing if x is on the "left" of the point and vectors that make up the 3 triangle lines (ccw)
if ( dotproduct( crossproduct(p1_-p0_, x-p0_), n_ ) >= 0.0f &&
dotproduct( crossproduct(p2_-p1_, x-p1_), n_ ) >= 0.0f &&
dotproduct( crossproduct(p0_-p2_, x-p2_), n_ ) >= 0.0f )
{
return t;
}
// otherwise the point x is not inside the triangle
else
{
return 0.0f;
}
}
/**
* judge this t is a tangent point or not where o is the new point
*/
bool isTangentPoint(tuple *o, tuple *prevous, tuple *current, tuple *nextone) {
int product1 = crossproduct(o, prevous, current);
int product2 = crossproduct(o, nextone, current);
int multiple = 1;
if ((product2 > 0 && product1 > 0) || (product2 < 0 && product1 < 0)) {
multiple = 1;
} else {
multiple = -1;
}
if (product1 == 0 || product2 == 0) {
if (product1 == 0 && product2 == 0) {
return false ;
} else {
return true ;
}
} else if (multiple > 0) {
return true ;
} else {
return false ;
}
}
void Camera::LookAtLH(Vector3 eye, Vector3 lookat, Vector3 up ){
Vector3 xaxis, yaxis, zaxis;
normalize( up );
zaxis.x = lookat.x-eye.x; zaxis.y = lookat.y-eye.y; zaxis.z = lookat.z-eye.z;
normalize( zaxis );
crossproduct( xaxis, up, zaxis );
normalize( xaxis );
crossproduct( yaxis, zaxis, xaxis );
matViewMatrix._11 = xaxis.x; matViewMatrix._12 = yaxis.x;
matViewMatrix._13 = zaxis.x; matViewMatrix._14 = 0.0f;
matViewMatrix._21 = xaxis.y; matViewMatrix._22 = yaxis.y;
matViewMatrix._23 = zaxis.y; matViewMatrix._24 = 0.0f;
matViewMatrix._31 = xaxis.z; matViewMatrix._32 = yaxis.z;
matViewMatrix._33 = zaxis.z; matViewMatrix._34 = 0.0f;
matViewMatrix._41 = -dot(xaxis,eye); matViewMatrix._42 = -dot(yaxis,eye);
matViewMatrix._43 = -dot(zaxis,eye); matViewMatrix._44 = 1.0f;
up_dir = yaxis;
right_dir = xaxis;
forward_dir = zaxis;
}
void
compute_angular_forces()
{
int i, j, k;
struct vertex *u, *v, *w;
struct point dvu, dvw, normal;
double fact, s;
s = 0.5 * sin((M_PI - bestangle) / 2.0);
for (i = 1; i <= nvertices; i++) {
u = &(vertices[i]);
for (j = 0; j + 1 < u->valency; j++) {
v = &(vertices[u->adj[j]]);
for (k = j + 1; k < u->valency; k++) {
w = &(vertices[u->adj[k]]);
dvu = difference(u->pos, v->pos);
dvw = difference(w->pos, v->pos);
normal = crossproduct(dvw, dvu);
normal = crossproduct(normal, dvw);
fact = s * norm(dvw) / norm(normal);
u->disp.x += 0.05 * (0.5 * dvw.x + fact * normal.x - dvu.x);
u->disp.y += 0.05 * (0.5 * dvw.y + fact * normal.y - dvu.y);
u->disp.z += 0.05 * (0.5 * dvw.z + fact * normal.z - dvu.z);
}
}
}
}
/**
* @brief judge the point D whether is inside the triangle.
*
* @param A one point of triangle.
* @param B one point of triangle.
* @param C one point of triangle.
* @param D one point to judge whether it is inside the triangle.
*
* @return return 0 means not inside the triangle, else return 1 means inside in
* the triangle, otherwise return -1 means some error occur, such as invalid
* triangle.
*/
int point_inside_triangle_use_crossproduct(POINT A, POINT B, POINT C, POINT D) {
if (!triangle_verification(A, B, C)) { return -1; }
// order as A, B, C by min(y-axis, x-axis)
POINT* P[] = {&A, &B, &C};
quicksort_descend_yx(P, 0, 2);
// judge the cross product
return (crossproduct(A, B, D) >= 0 && crossproduct(B, C, D) >= 0
&& crossproduct(C, A, D) >= 0);
}
int insidetriangle(double xs, double ys, double xa, double ya,
double xb, double yb, double xf, double yf) {
double cscross, cbcross, cfcross, cacross;
cscross = crossproduct(xa-xs, ya-ys, xf-xs, yf-ys);
cbcross = crossproduct(xb-xs, yb-ys, xf-xs, yf-ys);
if ((cscross < 0.0 && cbcross > 0.0) ||
(cscross > 0.0 && cbcross < 0.0)) return 0;
cfcross = crossproduct(xb-xf, yb-yf, xs-xf, ys-yf);
cacross = crossproduct(xa-xf, ya-yf, xs-xf, ys-yf);
if ((cfcross < 0.0 && cacross > 0.0) ||
(cfcross > 0.0 && cacross < 0.0)) return 0;
return 1;
}
/***************************************************************************
//Calculate the dihedral between plane p1-p2-p3 and p2-p3-p4
//p1[x, y, z], p2[x, y, z], p3[x, y, z], p4[x, y, z]
***************************************************************************/
double Dihedral(double *p1, double *p2, double *p3, double *p4)
{
double vector1[3];
subtract(p1, p2, vector1);
double vector2[3];
subtract(p2, p3, vector2);
double vector3[3];
subtract(p3, p4, vector3);
double v1[3];
crossproduct(vector2, vector1, v1);
double v2[3];
crossproduct(vector3, vector2, v2);
norm (v1);
norm (v2);
double dihedral = innerproduct(v1, v2);
if (dihedral>1 && dihedral<1+EXTRA)
{
dihedral=1;
}
else if(dihedral<-1 && dihedral>-1-EXTRA)
{
dihedral=-1;
}
else if(dihedral>1+EXTRA || dihedral<-1-EXTRA)
{
cout<<"Error, double Dihedral()\n";
exit(-1);
}
double v5[3];
crossproduct(v2, v1, v5);
double direction = innerproduct(v5, vector2);
if (direction>0)
{
return (acos(dihedral)/PI)*180;
}
else
{
return -(acos(dihedral)/PI)*180;
}
}
void
gen_chararray(Dimset* dimset, Datalist* data, Bytebuffer* databuf, Datalist* fillsrc)
{
int ndims,lastunlim;
int fillchar = getfillchar(fillsrc);
size_t expectedsize,xproduct;
size_t unitsize;
ASSERT(bbLength(databuf) == 0);
ndims = dimset->ndims;
/* Find the last unlimited */
lastunlim = findlastunlimited(dimset);
if(lastunlim < 0) lastunlim = 0; /* pretend */
/* Compute crossproduct upto the last dimension,
starting at the last unlimited
*/
xproduct = crossproduct(dimset,lastunlim,ndims-1);
if(ndims == 0) {
unitsize = 1;
} else if(lastunlim == ndims-1) {/* last dimension is unlimited */
unitsize = 1;
} else { /* last dim is not unlimited */
unitsize = dimset->dimsyms[ndims-1]->dim.declsize;
}
expectedsize = (xproduct * unitsize);
gen_chararrayr(dimset,0,lastunlim,databuf,data,fillchar,unitsize,expectedsize);
}
static void segment (float w, float *x, float *y) {
int i, j;
float t, p[3], q[3], up[3] = {0, 0, 1}, m[3], epsilon = 0.01;
p[0] = x[0]; p[1] = y[1];
glNormal3f (0, 0, 1);
glBegin (GL_QUAD_STRIP);
for (i = 0; i <= CONFIG_RENDER_DETAIL; ++i) {
t = (float) i / CONFIG_RENDER_DETAIL;
bezier3 (t, x, y, p);
bezier3 (t + epsilon, x, y, q);
p[2] = terrain_h (p[0], p[1]);
q[2] = terrain_h (q[0], q[1]);
for (j = 0; j < len (q); ++j) q[j] -= p[j];
crossproduct (q, up, m);
normalize (m, len (m));
for (j = 0; j < len (m); ++j) p[j] -= w * m[j];
glTexCoord2f (0, t);
glVertex3fv (p);
for (j = 0; j < len (m); ++j) p[j] += 2*w * m[j];
glTexCoord2f (1, t);
glVertex3fv (p);
}
glEnd ();
}
void SRS::SolveAim(float psi_angle, Matrix R1)
{
float h1[3], N[3], angle;
Matrix S0, S1;
// Get the final hand position
evalcircle(c, u, v, radius, psi_angle, h1);
// Rotate ee_r1 to h1
crossproduct(N, ee_r1, h1);
unitize(N);
angle = angle_between_vectors(ee_r1, h1, N);
rotation_axis_to_matrix(N, angle, S0);
// Now rotate a0 to a
float a[3], a0[3];
vecsub(a, (float*)ee, h1);
unitize(a);
hmatmult(S1,Ry,S0);
vecmult0(a0, (float*)axis, S1);
cpvector(N, h1);
unitize(N);
angle = angle_between_vectors(a0, a, N);
rotation_axis_to_matrix(N, angle, S1);
hmatmult(R1, S0, S1);
}
/* Used only for structure field arrays*/
static void
generate_fieldarray(Symbol* basetype, NCConstant* con, Dimset* dimset,
Bytebuffer* codebuf, Datalist* filler, Generator* generator)
{
int i;
int chartype = (basetype->typ.typecode == NC_CHAR);
Datalist* data;
ASSERT(dimset->ndims > 0);
if(con != NULL && !isfillconst(con))
data = con->value.compoundv;
else
data = NULL;
if(chartype) {
/* Collect the char field in a separate buffer */
Bytebuffer* charbuf = bbNew();
gen_chararray(dimset,data,charbuf,filler);
generator->charconstant(generator,codebuf,charbuf);
bbFree(charbuf);
} else {
int uid;
size_t xproduct = crossproduct(dimset,0,0); /* compute total number of elements */
generator->listbegin(generator,LISTFIELDARRAY,xproduct,codebuf,&uid);
for(i=0;i<xproduct;i++) {
con = (data == NULL ? NULL : datalistith(data,i));
generator->list(generator,LISTFIELDARRAY,uid,i,codebuf);
generate_basetype(basetype,con,codebuf,NULL,generator);
}
generator->listend(generator,LISTFIELDARRAY,uid,i,codebuf);
}
}
//----------------------------------------------------------------------------
// Summary: compute the acceleration bias (state dependent) term, zeta
// Parameters: omega_inboard - angular velocity of the inboard link
// omega_curr - angular velocity of the current link
// Returns: zeta - spatial acceleration bias term
//----------------------------------------------------------------------------
void dmPrismaticLink::computeZeta(const CartesianVector omega_inboard,
const CartesianVector omega_curr,
SpatialVector zeta)
{
CartesianVector tmp, tmp1;
crossproduct(&omega_inboard[0], m_p, tmp);
crossproduct(&omega_inboard[0], tmp, tmp1);
rtxFromInboard(tmp1, &zeta[3]);
zeta[3] += 2.0*omega_curr[1]*m_qd;
zeta[4] += -2.0*omega_curr[0]*m_qd;
zeta[0] = 0.0;
zeta[1] = 0.0;
zeta[2] = 0.0;
}
float dist_point_line_nosqr(vector3d &v1,
const float &x, const float &y, const float &z,
const float &px, const float &py, const float &pz)
{
float v1_mag_nosqr=v1.mag_nosqr();
if(v1_mag_nosqr<=0) return 100000.0;
vector3d v2(px-x,py-y,pz-z);
return crossproduct(v1,v2).mag_nosqr()/v1_mag_nosqr;
}
float get_circle_equation(const float ee[3],
const float axis[3],
const float pos_axis[3],
float upper_len,
float lower_len,
float c[3],
float u[3],
float v[3],
float n[3])
{
float wn = norm((float *)ee);
float radius;
cpvector(n, ee);
unitize(n);
// Use law of cosines to get angle between first spherical joint
// and revolute joint
float alpha;
if (!law_of_cosines(wn, upper_len, lower_len, alpha))
return 0;
// center of circle (origin is location of first S joint)
vecscalarmult(c, n, _cos(alpha) * upper_len);
radius = _sin(alpha) * upper_len;
float temp[3];
//
// A little kludgy. If the goal is behind the joint instead
// of in front of it, we reverse the angle measurement by
// inverting the normal vector
//
if (DOT(n,pos_axis) < 0.0)
vecscalarmult(n,n,-1.0);
vecscalarmult(temp, n, DOT(axis,n));
vecsub(u, (float *)axis, temp);
unitize(u);
crossproduct(v, n, u);
#if 0
printf("Circle equation\n");
printf("c = [%lf,%lf,%lf]\n", c[0], c[1], c[2]);
printf("u = [%lf,%lf,%lf]\n", u[0], u[1], u[2]);
printf("v = [%lf,%lf,%lf]\n", v[0], v[1], v[2]);
printf("n = [%lf,%lf,%lf]\n", n[0], n[1], n[2]);
printf("r = %lf\n", radius);
#endif
return radius;
}
//
// Return the angle between two vectors u,v about the axis n
//
float angle_between_vectors(float u[3], float v[3], float n[3])
{
#if 0
float temp[3];
float up[3];
float vp[3];
cpvector(up, u);
cpvector(vp, v);
unitize(up);
unitize(vp);
crossproduct(temp, up, vp);
float mag = DOT(temp,n);
// Vectors are parallel at 0 or 180
if (mag*mag < 1e-8)
{
if (DOT(up,vp) < 0)
return M_PI;
else
return 0;
}
int sign = (mag > 0) ? 1 : -1;
float t = DOT(up,vp);
if (t > 1.0)
t = 1.0;
else if (t < -1.0)
t = -1.0;
return sign*acos(t);
#else
float up[3];
float vp[3];
float uv[3];
project_plane(up, u, n);
project_plane(vp, v, n);
crossproduct(uv, up, vp);
return atan2(DOT(n, uv), DOT(up, vp));
#endif
}
static void crossproduct_v(
double const * const a,
double const * const b,
double * const c )
{
crossproduct(
a[0], a[1], a[2],
b[0], b[1], b[2],
c, c+1, c+2 );
}
void generatepolygonnormal(float *normal,float *vertex1,float *vertex2,float *vertex3)
{
float vec[3],vec2[3];
subtractvectors(vec,vertex2,vertex1);
subtractvectors(vec2,vertex3,vertex1);
crossproduct(normal,vec,vec2);
normalizevector(normal,normal);
}
int lineintersecttriangle(float *intersectpoint,float *normal,float *scale,float *startpoint,float *endpoint,float *vertex1,float *vertex2,float *vertex3)
{
float vec[3],vec2[3],vec3[3];
subtractvectors(vec,endpoint,vertex1);
if (dotproduct(vec,normal)>0.0f)
return(0);
subtractvectors(vec,startpoint,vertex1);
if (dotproduct(vec,normal)<0.0f)
return(0);
subtractvectors(vec,vertex1,startpoint);
*scale=dotproduct(vec,normal);
subtractvectors(vec2,endpoint,startpoint);
*scale/=dotproduct(vec2,normal);
scaleaddvectors(intersectpoint,startpoint,vec2,*scale);
subtractvectors(vec,vertex2,vertex1);
subtractvectors(vec2,intersectpoint,vertex1);
crossproduct(vec3,vec,vec2);
if (dotproduct(vec3,normal)<0.0f)
return(0);
subtractvectors(vec,vertex3,vertex2);
subtractvectors(vec2,intersectpoint,vertex2);
crossproduct(vec3,vec,vec2);
if (dotproduct(vec3,normal)<0.0f)
return(0);
subtractvectors(vec,vertex1,vertex3);
subtractvectors(vec2,intersectpoint,vertex3);
crossproduct(vec3,vec,vec2);
if (dotproduct(vec3,normal)<0.0f)
return(0);
return(1);
}
//calculate dihedral between two plane(plane1: p1,p2,p3 plane2: p4,p5,p6), (-180,180)
double MyDihedral(const vector<double> &p1, const vector<double> &p2, const vector<double> &p3,
const vector<double> &p4, const vector<double> &p5, const vector<double> &p6)
{
double vector1[3];
Mysubtract(p1, p2, vector1);
double vector2[3];
Mysubtract(p2, p3, vector2);
double v1[3];
crossproduct(vector1, vector2, v1);
double vector3[3];
Mysubtract(p4, p5, vector3);
double vector4[3];
Mysubtract(p5, p6, vector4);
double v2[3];
crossproduct(vector3, vector4, v2);
norm(v1);norm(v2);
double dihedral=innerproduct(v1,v2);
return (acos(dihedral)/PI)*180;
}
RTTriangle::RTTriangle(point &p0, point &p1, point &p2, std::string name)
{
// set p0, p1, p2 points
p0_ = p0;
p1_ = p1;
p2_ = p2;
// calculate normal n that points out the front of triangle
n_ = crossproduct( (p1_ - p0_), (p2_ - p0_) );
normalize(&n_);
name_ = name;
}
void rotateorientation(float orientation[3][3],float *rotationvector,float rotationangle)
{
int count;
float pointtemp[3],cosnormal[3],sinnormal[3];
float axisnormal[3];
float vectorlengthvalue,dotproductvalue;
normalizevector(axisnormal,rotationvector);
if (fabs(rotationangle)<.003f)
return;
for (count=0;count<3;count++)
{
copyvector(pointtemp,orientation[count]);
crossproduct(sinnormal,axisnormal,pointtemp);
normalizevector(sinnormal,sinnormal);
crossproduct(cosnormal,sinnormal,axisnormal);
normalizevector(cosnormal,cosnormal);
dotproductvalue=dotproduct(axisnormal,pointtemp);
pointtemp[0]-=axisnormal[0]*dotproductvalue;
pointtemp[1]-=axisnormal[1]*dotproductvalue;
pointtemp[2]-=axisnormal[2]*dotproductvalue;
vectorlengthvalue=vectorlength(pointtemp);
orientation[count][0]=axisnormal[0]*dotproductvalue+cosnormal[0]*cos(rotationangle)*vectorlengthvalue+sinnormal[0]*sin(rotationangle)*vectorlengthvalue;
orientation[count][1]=axisnormal[1]*dotproductvalue+cosnormal[1]*cos(rotationangle)*vectorlengthvalue+sinnormal[1]*sin(rotationangle)*vectorlengthvalue;
orientation[count][2]=axisnormal[2]*dotproductvalue+cosnormal[2]*cos(rotationangle)*vectorlengthvalue+sinnormal[2]*sin(rotationangle)*vectorlengthvalue;
normalizevector(orientation[count],orientation[count]);
}
}
int parallel(double dx, double dy, double us, double vs) {
double dotd, dots, cross;
/* double sdot; */
dotd = dotproduct(dx, dy, dx, dy);
dots = dotproduct(us, vs, us, vs);
/* sdot = sqrt(us * us + vs * vs);
if (sdot == 0.0) {
saywhere(errout);
fprintf(errout, "Zero tangent direction\n"); return 1;
}
us = us / sdot; vs = vs / sdot; */
cross = crossproduct(dx, dy, us, vs);
/* if (cross < 0.0) cross = - cross; */
/* if (cross < depsilon) return 1; */
if (cross * cross < epsilon * epsilon * dotd * dots) return 1;
else return 0;
}
static void
move_right(double factor){
double n[3];
double forward_v[3];
double up_v[3];
forward_v[0] = camera->forward.x;
forward_v[1] = camera->forward.y;
forward_v[2] = camera->forward.z;
up_v[0] = camera->up.x;
up_v[1] = camera->up.y;
up_v[2] = camera->up.z;
crossproduct(n, forward_v, up_v);
normalize(n);
camera->eye[0] += n[0] * factor;
camera->eye[1] += n[1] * factor;
camera->eye[2] += n[2] * factor;
}
void Rotation( double &x, double &y, double &z)
{
double r = 180/(3.1429*10000);
double s=cos(r/2);
double u[3];
u[0]=sin(r/2)*0.0;
u[1]=sin(r/2)*1.0;
u[2]=sin(r/2)*0.0;
double tempX=x;
double tempZ=z;
double tempY=y;
double cross[3];
crossproduct(u[0],u[1],u[2],tempX,tempY,tempZ, cross);
x = 2.0*DotProduct(u[0],u[1],u[2],tempX,tempY,tempZ) * u[0] + (s*s - DotProduct(u[0],u[1],u[2],u[0],u[1],u[2])) * tempX+ 2.0 *s*cross[0];
y = 2.0*DotProduct(u[0],u[1],u[2],tempX,tempY,tempZ) * u[1] + (s*s - DotProduct(u[0],u[1],u[2],u[0],u[1],u[2])) * tempY+ 2.0 *s*cross[1];
z = 2.0*DotProduct(u[0],u[1],u[2],tempX,tempY,tempZ) * u[2] + (s*s - DotProduct(u[0],u[1],u[2],u[0],u[1],u[2])) * tempZ+ 2.0 *s*cross[2];
}
static void
move_left(double factor){
double n[3];
double forward_v[3];
double up_v[3];
forward_v[0] = camera->forward.x;
forward_v[1] = camera->forward.y;
forward_v[2] = camera->forward.z;
up_v[0] = camera->up.x;
up_v[1] = camera->up.y;
up_v[2] = camera->up.z;
crossproduct(n, up_v, forward_v);
normalize(n);
//printf("n=(%f %f %f)\n", n[0], n[1], n[2]);
camera->eye[0] += n[0] * factor;
camera->eye[1] += n[1] * factor;
camera->eye[2] += n[2] * factor;
}
static void
gen_leafchararray(Dimset* dimset, int dimindex, Datalist* data,
Bytebuffer* charbuf, int fillchar)
{
int i;
size_t expectedsize,xproduct,unitsize;
int rank = rankfor(dimset);
ASSERT(bbLength(charbuf) == 0);
ASSERT((findlastunlimited(dimset) == rank
|| findlastunlimited(dimset) == dimindex));
/*
There are a number of special cases that must be
considered, mostly driven by the need to keep consistent
with ncgen3. These cases are driven by the number of
dimensions, which dimensions are unlimited (if any), etc.
The general rule is based on the size of the last
dimension, we compute the required size (after padding)
of each string constant. Expected size is then the size
of concat of the string constants after padding.
There is another special case used for back compatability with ncgen3.
In the datalist, all sequences of character constants (i.e. 'X')
are concatenated into a single string; the result, however is not
concatenated with any trailing or leading string (with double quotes).
*/
/* Rebuild the datalist to merge 'x' constants */
{
int i,cccount = 0;
/* Do initial walk */
for(i=0;i<datalistlen(data);i++) {
NCConstant* con = datalistith(data,i);
if(consttype(con) == NC_CHAR || consttype(con) == NC_BYTE) {
cccount++;
}
}
if(cccount > 1) {
char* accum = (char*)malloc(cccount+1);
int len = 0;
Datalist* newlist = builddatalist(datalistlen(data));
int lineno = 0;
NCConstant* con;
for(i=0;i<datalistlen(data);i++) {
con = datalistith(data,i);
if(consttype(con) == NC_CHAR || consttype(con) == NC_BYTE) {
if(len == 0)
lineno = constline(con);
accum[len] = con->value.charv;
len++;
} else {
if(len > 0) {
con = makeconst(lineno,len,accum);
len = 0;
lineno = 0;
}
dlappend(newlist,con);
}
}
/* deal with any unclosed strings */
if(len > 0) {
con = makeconst(lineno,len,accum);
len = 0;
lineno = 0;
dlappend(newlist,con);
}
free(accum);
data = newlist;
}
}
/* Compute crossproduct upto (but not includng) the last dimension */
xproduct = crossproduct(dimset,dimindex,rank-1);
/* Start casing it out */
if(rank == 0) {
unitsize = 1;
expectedsize = (xproduct * unitsize);
} else if(rank == 1) {
unitsize = 1;
expectedsize = (xproduct * declsizefor(dimset,rank-1));
} else if(isunlimited(dimset,rank-1)) {/* last dimension is unlimited */
unitsize = 1;
expectedsize = (xproduct*declsizefor(dimset,rank-1));
} else { /* rank > 0 && last dim is not unlimited */
unitsize = declsizefor(dimset,rank-1);
expectedsize = (xproduct * unitsize);
}
for(i=0;i<data->length;i++) {
NCConstant* c = datalistith(data,i);
ASSERT(!islistconst(c));
if(isstringable(c->nctype)) {
int j;
size_t constsize;
constsize = gen_charconstant(c,charbuf,fillchar);
if(constsize == 0 || constsize % unitsize > 0) {
size_t padsize = unitsize - (constsize % unitsize);
for(j=0;j<padsize;j++) bbAppend(charbuf,fillchar);
}
} else {
semwarn(constline(c),"Encountered non-string and non-char constant in datalist; ignored");
}
}
/* If |databuf| > expectedsize, complain: exception is zero length */
if(bbLength(charbuf) == 0 && expectedsize == 1) {
/* this is okay */
} else if(bbLength(charbuf) > expectedsize) {
semwarn(data->data[0].lineno,"character data list too long; expected %d character constant, found %d: ",expectedsize,bbLength(charbuf));
} else {
size_t bufsize = bbLength(charbuf);
/* Pad to size dimproduct size */
if(bufsize % expectedsize > 0) {
size_t padsize = expectedsize - (bufsize % expectedsize);
for(i=0;i<padsize;i++) bbAppend(charbuf,fillchar);
}
}
}
/**
* judge whether this new point inside the convex or not.
* use the o(n) search method
* if return ==0 this tuple inside the convex and this tupe can join this convex
* if return ==1 this tuple outside of convex and it also can join this convex
* if return ==2 this tuple outside of convex and it can not join this convex because the longest
* if return ==3 this tuple outside of convex and the it has some overlap with the current group
* distance is bigger than the eps
*/
int insideConvexAndInsideBound(convexhull *convex, tuple *newpoint, int eps) {
if (convex == NULL ) {
return 2;
}
if (convex->numofconvex == 0) {
return 1;
} else if (convex->numofconvex == 1) {
double distance = L2distance(convex->convexheader, newpoint);
if (distance <= eps) {
return 1;
} else {
return 2;
}
}
tuple *current = NULL;
if (convex->convexheader != NULL )
current = convex->convexheader->next;
int count1 = 0;
int count2 = 0;
double longest = 0;
int i = 0;
bool overlap=false;
while (current != NULL && i < convex->numofconvex) {
tuple *next = current->next;
if (next == NULL ) {
break;
}
double distance = L2distance(newpoint, current);
if(distance==0)
{
return 0;
}
if (distance > longest) {
if(longest<=eps)
{
overlap=true;
}
longest = distance;
if (longest > eps&&overlap==true) {
return 3;
}
}
int value = crossproduct(newpoint, current, next);
if (value > 0) {
count1++;
} else if (value < 0) {
count2++;
}
current = current->next;
i++;
}
if (0 == count1 || 0 == count2) {
if (convex->numofconvex == 2)
return 1;
else
return 0;
} else //this tuple is in the outside area
{
if (longest < eps)
return 1;
else{
if(overlap==true)
{
return 3;
}else
{
return 2;
}
}
}
}
// set origin of pResult to the intersecting point
// of pL1, and pL2 assuming they are not skew
int FindIntersection( PVECTOR presult,
PVECTOR s1, PVECTOR o1,
PVECTOR s2, PVECTOR o2 )
{
VECTOR R1, R2, denoms;
RCOORD t1, t2, denom;
#define a (o1[0])
#define b (o1[1])
#define c (o1[2])
#define d (o2[0])
#define e (o2[1])
#define f (o2[2])
#define na (s1[0])
#define nb (s1[1])
#define nc (s1[2])
#define nd (s2[0])
#define ne (s2[1])
#define nf (s2[2])
crossproduct( denoms, s1, s2 );
denom = denoms[2];
// denom = ( nd * nb ) - ( ne * na );
if( NearZero( denom ) )
{
denom = denoms[1];
// denom = ( nd * nc ) - (nf * na );
if( NearZero( denom ) )
{
denom = denoms[0];
// denom = ( ne * nc ) - ( nb * nf );
if( NearZero( denom ) )
{
SetPoint( presult, VectorConst_0 );
return FALSE;
}
else
{
t1 = ( ne * ( f - c ) + nf * ( e - b ) ) / -denom;
t2 = ( nb * ( c - f ) + nc * ( b - e ) ) / denom;
}
}
else
{
t1 = ( nd * ( f - c ) + nf * ( d - a ) ) / -denom;
t2 = ( na * ( c - f ) + nc * ( a - d ) ) / denom;
}
}
else
{
t1 = ( nd * ( e - b ) + ne * ( d - a ) ) / -denom;
t2 = ( na * ( b - e ) + nb * ( a - d ) ) / denom;
}
scale( R1, s1, t1 );
add ( R1, R1 , o1 );
scale( R2, s2, t2 );
add ( R2, R2 , o2 );
if( ( !COMPARE(R1[0] , R2[0]) ) ||
( !COMPARE(R1[1] , R2[1]) ) ||
( !COMPARE(R1[2] , R2[2]) ) )
{
SetPoint( presult, VectorConst_0 );
return FALSE;
}
SetPoint( presult, R1);
return TRUE;
#undef a
#undef b
#undef c
#undef d
#undef e
#undef f
#undef na
#undef nb
#undef nc
#undef nd
#undef ne
#undef nf
}
int FindIntersectionTime( RCOORD *pT1, PVECTOR s1, PVECTOR o1
, RCOORD *pT2, PVECTOR s2, PVECTOR o2 )
{
VECTOR R1, R2, denoms;
RCOORD t1, t2, denom;
#define a (o1[0])
#define b (o1[1])
#define c (o1[2])
#define d (o2[0])
#define e (o2[1])
#define f (o2[2])
#define na (s1[0])
#define nb (s1[1])
#define nc (s1[2])
#define nd (s2[0])
#define ne (s2[1])
#define nf (s2[2])
crossproduct(denoms, s1, s2 ); // - result...
denom = denoms[2];
// denom = ( nd * nb ) - ( ne * na );
if( NearZero( denom ) )
{
denom = denoms[1];
// denom = ( nd * nc ) - (nf * na );
if( NearZero( denom ) )
{
denom = denoms[0];
// denom = ( ne * nc ) - ( nb * nf );
if( NearZero( denom ) )
{
#ifdef FULL_DEBUG
sprintf( (char*)byBuffer,"Bad!-------------------------------------------\n");
Log( (char*)byBuffer );
#endif
return FALSE;
}
else
{
DebugBreak();
t1 = ( ne * ( c - f ) + nf * ( b - e ) ) / denom;
t2 = ( nb * ( c - f ) + nc * ( b - e ) ) / denom;
}
}
else
{
DebugBreak();
t1 = ( nd * ( c - f ) + nf * ( d - a ) ) / denom;
t2 = ( na * ( c - f ) + nc * ( d - a ) ) / denom;
}
}
else
{
// this one has been tested.......
t1 = ( nd * ( b - e ) + ne * ( d - a ) ) / denom;
t2 = ( na * ( b - e ) + nb * ( d - a ) ) / denom;
}
R1[0] = a + na * t1;
R1[1] = b + nb * t1;
R1[2] = c + nc * t1;
R2[0] = d + nd * t2;
R2[1] = e + ne * t2;
R2[2] = f + nf * t2;
if( ( !COMPARE(R1[0],R2[0]) ) ||
( !COMPARE(R1[1],R2[1]) ) ||
( !COMPARE(R1[2],R2[2]) ) )
{
return FALSE;
}
*pT2 = t2;
*pT1 = t1;
return TRUE;
#undef a
#undef b
#undef c
#undef d
#undef e
#undef f
#undef na
#undef nb
#undef nc
#undef nd
#undef ne
#undef nf
}
