void scanConverter(GzRender *render, GzCoord *vertexList)
{
GzCoord top, mid, btm;
GzCoord vtx1, vtx2, vtx3;
/* Find a bounding box of the tri, and sort 3 verts */
/* Find max and min x values */
float minx, miny, maxx, maxy;
if (vertexList[0][0] >= vertexList[1][0])
{
maxx = vertexList[0][0];
minx = vertexList[1][0];
}
else
{
maxx = vertexList[1][0];
minx = vertexList[0][0];
}
if (vertexList[2][0] > maxx) maxx = vertexList[2][0];
if (vertexList[2][0] < minx) minx = vertexList[2][0];
/* Find max and min y values*/
if (vertexList[0][1] >= vertexList[1][1])
{
top[0] = vertexList[0][0]; top[1] = vertexList[0][1]; top[2] = vertexList[0][2];
btm[0] = vertexList[1][0]; btm[1] = vertexList[1][1]; btm[2] = vertexList[1][2];
}
else
{
top[0] = vertexList[1][0]; top[1] = vertexList[1][1]; top[2] = vertexList[1][2];
btm[0] = vertexList[0][0]; btm[1] = vertexList[0][1]; btm[2] = vertexList[0][2];
}
if (vertexList[2][1] > top[1])
{
mid[0] = top[0]; mid[1] = top[1]; mid[2] = top[2];
top[0] = vertexList[2][0]; top[1] = vertexList[2][1]; top[2] = vertexList[2][2];
}
else if (vertexList[2][1] < btm[1])
{
mid[0] = btm[0]; mid[1] = btm[1]; mid[2] = btm[2];
btm[0] = vertexList[2][0]; btm[1] = vertexList[2][1]; btm[2] = vertexList[2][2];
}
else
{
mid[0] = vertexList[2][0]; mid[1] = vertexList[2][1]; mid[2] = vertexList[2][2];
}
maxx = ceil(maxx);
maxy = ceil(top[1]);
minx = floor(minx);
miny = floor(btm[1]);
if (top[1] == mid[1]) // Top edge exits
{
if (top[0] < mid[0])
{
vtx1[0] = top[0]; vtx1[1] = top[1]; vtx1[2] = top[2];
vtx2[0] = mid[0]; vtx2[1] = mid[1]; vtx2[2] = mid[2];
}
else
{
vtx2[0] = top[0]; vtx2[1] = top[1]; vtx2[2] = top[2];
vtx1[0] = mid[0]; vtx1[1] = mid[1]; vtx1[2] = mid[2];
}
vtx3[0] = top[0]; vtx1[1] = top[1]; vtx1[2] = top[2];
}
else if (btm[1] == mid[1]) // Bottom edge exits
{
if (btm[0] < mid[0])
{
vtx3[0] = btm[0]; vtx3[1] = btm[1]; vtx3[2] = btm[2];
vtx2[0] = mid[0]; vtx2[1] = mid[1]; vtx2[2] = mid[2];
}
else
{
vtx2[0] = btm[0]; vtx2[1] = btm[1]; vtx2[2] = btm[2];
vtx3[0] = mid[0]; vtx3[1] = mid[1]; vtx3[2] = mid[2];
}
vtx1[0] = top[0]; vtx1[1] = top[1]; vtx1[2] = top[2];
}
else
{
float deltax = top[0] - btm[0];
float deltay = top[1] - btm[1];
float slope = deltax / deltay;
float x = mid[1] * slope - top[1] * slope + top[0];
vtx1[0] = top[0]; vtx1[1] = top[1]; vtx1[2] = top[2];
if (x < mid[0])
{
vtx2[0] = mid[0]; vtx2[1] = mid[1]; vtx2[2] = mid[2];
vtx3[0] = btm[0]; vtx3[1] = btm[1]; vtx3[2] = btm[2];
}
else
{
vtx2[0] = btm[0]; vtx2[1] = btm[1]; vtx2[2] = btm[2];
vtx3[0] = mid[0]; vtx3[1] = mid[1]; vtx3[2] = mid[2];
}
}
for (int i = (int)minx; i < (int)maxx; i++)
{
for (int j = (int)miny; j < (int)maxy; j++)
{
GzCoord point;
point[0] = i;
point[1] = j;
point[2] = 0;
if (LEE(vtx1, vtx2, point) >= 0 && LEE(vtx2, vtx3, point) >= 0 && LEE(vtx3, vtx1, point) >= 0)
{
/* Interpolate Z */
float vector1[3], vector2[3];
float *result = (float*)malloc(sizeof(float)* 3);
vector1[0] = vtx1[0] - vtx2[0]; vector1[1] = vtx1[1] - vtx2[1]; vector1[2] = vtx1[2] - vtx2[2];
vector2[0] = vtx3[0] - vtx2[0]; vector2[1] = vtx3[1] - vtx2[1]; vector2[2] = vtx3[2] - vtx2[2];
result = crossProduct(vector1, vector2);
if (result[0] * vtx2[0] + result[1] * vtx2[1] + result[2] * vtx2[2] == result[0] * vtx3[0] + result[1] * vtx3[1] + result[2] * vtx3[2])
int a = 3;
point[2] = (result[0] * vtx1[0] + result[1] * vtx1[1] + result[2] * vtx1[2] - result[0] * i - result[1] * j) / result[2];
///* Do the transformation */
//transform(render, point);
Rasterize(render->display, point[0], point[1],
ctoi(render->flatcolor[0]),
ctoi(render->flatcolor[1]),
ctoi(render->flatcolor[2]),
1, point[2]);
}
}
}
}
void vertexshade(void){
int nx,ny;
float cx,cy,cz;
for (nx=0;nx<n;nx++){
for (ny=0;ny<n;ny++){
cpx[n*nx+ny] = 0;
cpz[n*nx+ny] = 0;
cpy[n*nx+ny] = 0;
}
}
nx=0;ny=0;
crossProduct(x[nx*n+ny],y[nx*n+ny],z[nx*n+ny],x[nx*n+ny+1],y[nx*n+ny+1],z[nx*n+ny+1],x[(nx+1)*n+ny],y[(nx+1)*n+ny],z[(nx+1)*n+ny],cx,cy,cz);
cpx[n*nx+ny] -= cx;
cpz[n*nx+ny] -= cy;
cpy[n*nx+ny] -= cz;
ny=n-1;
crossProduct(x[nx*n+ny],y[nx*n+ny],z[nx*n+ny],x[(nx+1)*n+ny],y[(nx+1)*n+ny],z[(nx+1)*n+ny],x[nx*n+ny-1],y[nx*n+ny-1],z[nx*n+ny-1],cx,cy,cz);
cpx[n*nx+ny] -= cx;
cpz[n*nx+ny] -= cy;
cpy[n*nx+ny] -= cz;
nx=n-1;
crossProduct(x[nx*n+ny],y[nx*n+ny],z[nx*n+ny],x[nx*n+ny-1],y[nx*n+ny-1],z[nx*n+ny-1],x[(nx-1)*n+ny],y[(nx-1)*n+ny],z[(nx-1)*n+ny],cx,cy,cz);
cpx[n*nx+ny] -= cx;
cpz[n*nx+ny] -= cy;
cpy[n*nx+ny] -= cz;
ny=0;
crossProduct(x[nx*n+ny],y[nx*n+ny],z[nx*n+ny],x[(nx-1)*n+ny],y[(nx-1)*n+ny],z[(nx-1)*n+ny],x[nx*n+ny+1],y[nx*n+ny+1],z[nx*n+ny+1],cx,cy,cz);
cpx[n*nx+ny] -= cx;
cpz[n*nx+ny] -= cy;
cpy[n*nx+ny] -= cz;
for (ny=1;ny<n-1;ny++){
nx = 0;
crossProduct(x[nx*n+ny],y[nx*n+ny],z[nx*n+ny],x[nx*n+ny+1],y[nx*n+ny+1],z[nx*n+ny+1],x[(nx+1)*n+ny],y[(nx+1)*n+ny],z[(nx+1)*n+ny],cx,cy,cz);
cpx[n*nx+ny] -= cx;
cpz[n*nx+ny] -= cy;
cpy[n*nx+ny] -= cz;
crossProduct(x[nx*n+ny],y[nx*n+ny],z[nx*n+ny],x[(nx+1)*n+ny],y[(nx+1)*n+ny],z[(nx+1)*n+ny],x[nx*n+ny-1],y[nx*n+ny-1],z[nx*n+ny-1],cx,cy,cz);
cpx[n*nx+ny] -= cx;
cpz[n*nx+ny] -= cy;
cpy[n*nx+ny] -= cz;
nx = n-1;
crossProduct(x[nx*n+ny],y[nx*n+ny],z[nx*n+ny],x[(nx-1)*n+ny],y[(nx-1)*n+ny],z[(nx-1)*n+ny],x[nx*n+ny+1],y[nx*n+ny+1],z[nx*n+ny+1],cx,cy,cz);
cpx[n*nx+ny] -= cx;
cpz[n*nx+ny] -= cy;
cpy[n*nx+ny] -= cz;
crossProduct(x[nx*n+ny],y[nx*n+ny],z[nx*n+ny],x[nx*n+ny-1],y[nx*n+ny-1],z[nx*n+ny-1],x[(nx-1)*n+ny],y[(nx-1)*n+ny],z[(nx-1)*n+ny],cx,cy,cz);
cpx[n*nx+ny] -= cx;
cpz[n*nx+ny] -= cy;
cpy[n*nx+ny] -= cz;
}
for (nx=1;nx<n-1;nx++){
ny = 0;
crossProduct(x[nx*n+ny],y[nx*n+ny],z[nx*n+ny],x[(nx-1)*n+ny],y[(nx-1)*n+ny],z[(nx-1)*n+ny],x[nx*n+ny+1],y[nx*n+ny+1],z[nx*n+ny+1],cx,cy,cz);
cpx[n*nx+ny] -= cx;
cpz[n*nx+ny] -= cy;
cpy[n*nx+ny] -= cz;
crossProduct(x[nx*n+ny],y[nx*n+ny],z[nx*n+ny],x[nx*n+ny+1],y[nx*n+ny+1],z[nx*n+ny+1],x[(nx+1)*n+ny],y[(nx+1)*n+ny],z[(nx+1)*n+ny],cx,cy,cz);
cpx[n*nx+ny] -= cx;
cpz[n*nx+ny] -= cy;
cpy[n*nx+ny] -= cz;
ny = n-1;
crossProduct(x[nx*n+ny],y[nx*n+ny],z[nx*n+ny],x[(nx+1)*n+ny],y[(nx+1)*n+ny],z[(nx+1)*n+ny],x[nx*n+ny-1],y[nx*n+ny-1],z[nx*n+ny-1],cx,cy,cz);
cpx[n*nx+ny] -= cx;
cpz[n*nx+ny] -= cy;
cpy[n*nx+ny] -= cz;
crossProduct(x[nx*n+ny],y[nx*n+ny],z[nx*n+ny],x[nx*n+ny-1],y[nx*n+ny-1],z[nx*n+ny-1],x[(nx-1)*n+ny],y[(nx-1)*n+ny],z[(nx-1)*n+ny],cx,cy,cz);
cpx[n*nx+ny] -= cx;
cpz[n*nx+ny] -= cy;
cpy[n*nx+ny] -= cz;
}
for (nx=1;nx<n-1;nx++){
for (ny=1;ny<n-1;ny++){
crossProduct(x[nx*n+ny],y[nx*n+ny],z[nx*n+ny],x[(nx-1)*n+ny],y[(nx-1)*n+ny],z[(nx-1)*n+ny],x[nx*n+ny+1],y[nx*n+ny+1],z[nx*n+ny+1],cx,cy,cz);
cpx[n*nx+ny] -= cx;
cpz[n*nx+ny] -= cy;
cpy[n*nx+ny] -= cz;
crossProduct(x[nx*n+ny],y[nx*n+ny],z[nx*n+ny],x[nx*n+ny+1],y[nx*n+ny+1],z[nx*n+ny+1],x[(nx+1)*n+ny],y[(nx+1)*n+ny],z[(nx+1)*n+ny],cx,cy,cz);
cpx[n*nx+ny] -= cx;
cpz[n*nx+ny] -= cy;
cpy[n*nx+ny] -= cz; crossProduct(x[nx*n+ny],y[nx*n+ny],z[nx*n+ny],x[(nx+1)*n+ny],y[(nx+1)*n+ny],z[(nx+1)*n+ny],x[nx*n+ny-1],y[nx*n+ny-1],z[nx*n+ny-1],cx,cy,cz);
cpx[n*nx+ny] -= cx;
cpz[n*nx+ny] -= cy;
cpy[n*nx+ny] -= cz;
crossProduct(x[nx*n+ny],y[nx*n+ny],z[nx*n+ny],x[nx*n+ny-1],y[nx*n+ny-1],z[nx*n+ny-1],x[(nx-1)*n+ny],y[(nx-1)*n+ny],z[(nx-1)*n+ny],cx,cy,cz);
cpx[n*nx+ny] -= cx;
cpz[n*nx+ny] -= cy;
cpy[n*nx+ny] -= cz;
}
}
for (nx=0;nx<n-1;nx++){
for (ny=0;ny<n-1;ny++){
glBegin(GL_TRIANGLES);
glNormal3f(cpx[nx*n+ny],cpy[nx*n+ny],cpz[nx*n+ny]);
glVertex3f(x[nx*n+ny],y[nx*n+ny],z[nx*n+ny]);
glNormal3f(cpx[(nx+1)*n+ny],cpy[(nx+1)*n+ny],cpz[(nx+1)*n+ny]);
glVertex3f(x[(nx+1)*n+ny],y[(nx+1)*n+ny],z[(nx+1)*n+ny]);
glNormal3f(cpx[nx*n+ny+1],cpy[nx*n+ny+1],cpz[nx*n+ny+1]);
glVertex3f(x[nx*n+ny+1],y[nx*n+ny+1],z[nx*n+ny+1]);
glNormal3f(cpx[(nx+1)*n+ny],cpy[(nx+1)*n+ny],cpz[(nx+1)*n+ny]);
glVertex3f(x[(nx+1)*n+ny],y[(nx+1)*n+ny],z[(nx+1)*n+ny]);
glNormal3f(cpx[nx*n+ny+1],cpy[nx*n+ny+1],cpz[nx*n+ny+1]);
glVertex3f(x[nx*n+ny+1],y[nx*n+ny+1],z[nx*n+ny+1]);
glNormal3f(cpx[(nx+1)*n+ny+1],cpy[(nx+1)*n+ny+1],cpz[(nx+1)*n+ny+1]);
glVertex3f(x[(nx+1)*n+ny+1],y[(nx+1)*n+ny+1],z[(nx+1)*n+ny+1]);
glEnd();
}
}
}
/*!
Returns the normal vector of a plane defined by vectors \a v1 and \a v2,
normalized to be a unit vector.
Use crossProduct() to compute the cross-product of \a v1 and \a v2 if you
do not need the result to be normalized to a unit vector.
\sa crossProduct(), distanceToPlane()
*/
QVector3D QVector3D::normal(const QVector3D& v1, const QVector3D& v2)
{
return crossProduct(v1, v2).normalized();
}
void Frustum::computePerspective(const Vec3& position,
const Vec3& direction,
const Vec3& up,
float fov,
float ratio,
float near_distance,
float far_distance)
{
ASSERT(near_distance > 0);
ASSERT(far_distance > 0);
ASSERT(near_distance < far_distance);
ASSERT(fov > 0);
ASSERT(ratio > 0);
float tang = (float)tan(fov * 0.5f);
float near_height = near_distance * tang;
float near_width = near_height * ratio;
Vec3 z = direction;
z.normalize();
Vec3 x = crossProduct(up, z);
x.normalize();
Vec3 y = crossProduct(z, x);
Vec3 near_center = position - z * near_distance;
Vec3 far_center = position - z * far_distance;
m_center = position - z * ((near_distance + far_distance)* 0.5f);
m_plane[(uint32)Sides::NEAR_PLANE].set(-z, near_center);
m_plane[(uint32)Sides::FAR_PLANE].set(z, far_center);
Vec3 aux = (near_center + y * near_height) - position;
aux.normalize();
Vec3 normal = crossProduct(aux, x);
m_plane[(uint32)Sides::TOP_PLANE].set(normal, near_center + y * near_height);
aux = (near_center - y * near_height) - position;
aux.normalize();
normal = crossProduct(x, aux);
m_plane[(uint32)Sides::BOTTOM_PLANE].set(normal, near_center - y * near_height);
aux = (near_center - x * near_width) - position;
aux.normalize();
normal = crossProduct(aux, y);
m_plane[(uint32)Sides::LEFT_PLANE].set(normal, near_center - x * near_width);
aux = (near_center + x * near_width) - position;
aux.normalize();
normal = crossProduct(y, aux);
m_plane[(uint32)Sides::RIGHT_PLANE].set(normal, near_center + x * near_width);
float far_height = far_distance * tang;
float far_width = far_height * ratio;
Vec3 corner1 = near_center + x * near_width + y * near_height;
Vec3 corner2 = far_center + x * far_width + y * far_height;
float size = (corner1 - corner2).length();
size = Math::maxValue(sqrt(far_width * far_width * 4 + far_height * far_height * 4), size);
m_radius = size * 0.5f;
m_position = position;
m_direction = direction;
m_up = up;
m_fov = fov;
m_ratio = ratio;
m_near_distance = near_distance;
m_far_distance = far_distance;
}
double areaTriangle(const Vector3D& vecA, const Vector3D& vecB, const Vector3D& vecC) {
return 0.5 * crossProduct(vecA-vecB, vecC-vecB).Length();
}
/// Draw the shadow for a shape
static void pie_DrawShadow(iIMDShape *shape, int flag, int flag_data, Vector3f* light)
{
unsigned int i, j, n;
Vector3f *pVertices;
iIMDPoly *pPolys;
unsigned int edge_count = 0;
static EDGE *edgelist = NULL;
static unsigned int edgelistsize = 256;
EDGE *drawlist = NULL;
if(!edgelist)
{
edgelist = (EDGE*)malloc(sizeof(EDGE)*edgelistsize);
}
pVertices = shape->points;
if( flag & pie_STATIC_SHADOW && shape->shadowEdgeList )
{
drawlist = shape->shadowEdgeList;
edge_count = shape->nShadowEdges;
}
else
{
for (i = 0, pPolys = shape->polys; i < shape->npolys; ++i, ++pPolys) {
Vector3f p[3];
int current, first;
for(j = 0; j < 3; j++)
{
current = pPolys->pindex[j];
p[j] = Vector3f(pVertices[current].x, scale_y(pVertices[current].y, flag, flag_data), pVertices[current].z);
}
Vector3f normal = crossProduct(p[2] - p[0], p[1] - p[0]);
if (normal * *light > 0)
{
first = pPolys->pindex[0];
for (n = 1; n < pPolys->npnts; n++) {
// link to the previous vertex
addToEdgeList(pPolys->pindex[n-1], pPolys->pindex[n], edgelist, &edge_count, pVertices);
// check if the edgelist is still large enough
if(edge_count >= edgelistsize-1)
{
// enlarge
EDGE* newstack;
edgelistsize *= 2;
newstack = (EDGE *)realloc(edgelist, sizeof(EDGE) * edgelistsize);
if (newstack == NULL)
{
debug(LOG_FATAL, "pie_DrawShadow: Out of memory!");
abort();
return;
}
edgelist = newstack;
debug(LOG_WARNING, "new edge list size: %u", edgelistsize);
}
}
// back to the first
addToEdgeList(pPolys->pindex[pPolys->npnts-1], first, edgelist, &edge_count, pVertices);
}
}
//debug(LOG_WARNING, "we have %i edges", edge_count);
drawlist = edgelist;
if(flag & pie_STATIC_SHADOW)
{
// first compact the current edgelist
for(i = 0, j = 0; i < edge_count; i++)
{
if(edgelist[i].from < 0)
{
continue;
}
edgelist[j] = edgelist[i];
j++;
}
edge_count = j;
// then store it in the imd
shape->nShadowEdges = edge_count;
shape->shadowEdgeList = (EDGE *)realloc(shape->shadowEdgeList, sizeof(EDGE) * shape->nShadowEdges);
memcpy(shape->shadowEdgeList, edgelist, sizeof(EDGE) * shape->nShadowEdges);
}
}
// draw the shadow volume
glBegin(GL_QUADS);
glNormal3f(0.0, 1.0, 0.0);
for(i=0;i<edge_count;i++)
{
int a = drawlist[i].from, b = drawlist[i].to;
if(a < 0)
{
continue;
}
glVertex3f(pVertices[b].x, scale_y(pVertices[b].y, flag, flag_data), pVertices[b].z);
glVertex3f(pVertices[b].x+light->x, scale_y(pVertices[b].y, flag, flag_data)+light->y, pVertices[b].z+light->z);
glVertex3f(pVertices[a].x+light->x, scale_y(pVertices[a].y, flag, flag_data)+light->y, pVertices[a].z+light->z);
glVertex3f(pVertices[a].x, scale_y(pVertices[a].y, flag, flag_data), pVertices[a].z);
}
glEnd();
#ifdef SHOW_SHADOW_EDGES
glDisable(GL_DEPTH_TEST);
glColorMask(GL_TRUE, GL_TRUE, GL_TRUE, GL_TRUE);
glColor4ub(0xFF, 0, 0, 0xFF);
glBegin(GL_LINES);
for(i = 0; i < edge_count; i++)
{
int a = drawlist[i].from, b = drawlist[i].to;
if(a < 0)
{
continue;
}
glVertex3f(pVertices[b].x, scale_y(pVertices[b].y, flag, flag_data), pVertices[b].z);
glVertex3f(pVertices[a].x, scale_y(pVertices[a].y, flag, flag_data), pVertices[a].z);
}
glEnd();
glColorMask(GL_FALSE, GL_FALSE, GL_FALSE, GL_FALSE);
glEnable(GL_DEPTH_TEST);
#endif
}
bool F_OrientedHistogram::getFeatureVector(osvm_node *x, Slice3d* slice3d, int sid, int nsid)
{
node cs;
node cn;
node ct;
float v1[3];
float v2[3];
float v2p[3];
// B=(v1,vn,vn2) basis used to project vectors
float B[9];
int angleXY;
int angleXZ;
int idx = 0;
supernode* s = slice3d->getSupernode(sid);
supernode* n = slice3d->getSupernode(nsid);
s->getCenter(cs);
n->getCenter(cn);
// compute vector sn
v1[0] = cs.x - cn.x;
v1[1] = cs.y - cn.y;
v1[2] = cs.z - cn.z;
// Compute B basis
B[0] = v1[0];
B[1] = v1[1];
B[2] = v1[2];
B[3] = -v1[1];
B[4] = v1[0];
B[5] = 0;
crossProduct(&B[0], &B[3], &B[6]);
Histogram hist(nOrientation*nOrientation);
float angleToIdx = (float)nOrientation/360.0f;
vector < supernode* >* lNeighbors = &(s->neighbors);
supernode* t;
for(vector < supernode* >::iterator itN = lNeighbors->begin();
itN != lNeighbors->end(); itN++)
{
//if(t->id == n->id)
// continue;
t = *itN;
t->getCenter(ct);
// compute vector st
v2[0] = cs.x - ct.x;
v2[1] = cs.y - ct.y;
v2[2] = cs.z - ct.z;
/*
n1 = sqrt(v1[0]*v1[0]+v1[1]*v1[1]+v1[2]*v1[2]);
n2 = sqrt(v2[0]*v2[0]+v2[1]*v2[1]+v2[2]*v2[2]);
if(n1 != 0 && n2 != 0)
{
fCos = (v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2])/(n1*n2);
if(fCos < -1.0)
fCos = -1.0;
else
if(fCos > 1.0f)
fCos = 1.0f;
angle = acos(fCos)*180.0/PI;
crossProduct(v1,v2,vc);
fSin = l2Norm(vc, 3)/(l2Norm(v1, 3)*l2Norm(v2, 3));
if(fSin < -1.0)
fSin = -1.0;
else
if(fSin > 1.0f)
fSin = 1.0f;
printf("[F_OrientedHistogram] 1 angle=%d asin=%f\n",
angle,
fSin);
angle = asin(fSin)*180.0/PI;
printf("[F_OrientedHistogram] 2 fSin=%f, fCos=%f, angle=%d\n",
fSin,
fCos,
angle);
angle = atan2(fSin, fCos)*180.0/PI;
}
else
angle = 0;
idx = (int)(angle*angleToIdx+0.5);
*/
// Project v2 in B
matMulVec_3(B, v2, v2p);
// compute angle in x-y plane
angleXY = atan2(v2p[1], v2p[0])*180.0/PI;
angleXY += 180.0;
// compute angle in x-z plane
angleXZ = atan2(v2p[2], v2p[0])*180.0/PI;
angleXZ += 180.0;
if(angleXY > 360.0 || angleXZ > 360.0)
{
printf("[F_OrientedHistogram] Error : angleXY=%d > 360.0 || angleXZ=%d > 360.0\n", angleXY, angleXZ);
exit(-1);
}
idx = (int)(angleXY*angleToIdx)*nOrientation
+(int)(angleXZ*angleToIdx);
if(idx >= hist.nBins)
idx = hist.nBins - 1;
if(hist.histData[idx] < 0.1)
hist.histData[idx] = slice3d->getAvgIntensity(t->id);
else
hist.histData[idx] = (hist.histData[idx]+slice3d->getAvgIntensity(t->id))/2;
if(t->id == n->id)
{
printf("[F_OrientedHistogram] v2p=(%f,%f,%f)\n", v2p[0],v2p[1],v2p[2]);
printf("[F_OrientedHistogram] angleXY=%d angleXZ=%d angleToIdx=%f idx=%d bin[idx]=%f\n",
angleXY,angleXZ,angleToIdx,idx, hist.histData[idx]);
}
}
for(int i = 0;i < hist.nBins; i++)
x[i].value = hist.histData[i];
return true;
}
/*!
* This sub-routine computes the velocity vector, given a magnitude, a unit normal vector from the
* point on the surface where the regolith is lofted from, and the two conic angles which describe
* the velocity vector's direction relative to the normal vector. A backwards approach is used
* to go from the velocity vector in the final rotated intermediate frame back to the body fixed
* frame. More details are given in the thesis report and author's personal notes.
*
*/
void computeRegolithVelocityVector( std::vector< double > regolithPositionVector,
const double velocityMagnitude,
const double coneAngleAzimuth,
const double coneAngleDeclination,
std::vector< double > &unitNormalVector,
std::vector< double > ®olithVelocityVector )
{
// form the velocity vector, assuming that the intermediate frame's z-axis, on the surface of
// the asteroid, is along the final velocity vector
regolithVelocityVector[ 0 ] = 0.0;
regolithVelocityVector[ 1 ] = 0.0;
regolithVelocityVector[ 2 ] = velocityMagnitude;
// get the rotatin matrix to go from the rotated intermediate frame back to the initial
// frame where the z-axis was along the normal axis and the x-axis was pointing towards north
// direction
std::vector< std::vector< double > > zBasicRotationMatrix
{ { std::cos( coneAngleAzimuth ), std::sin( coneAngleAzimuth ), 0.0 },
{ -std::sin( coneAngleAzimuth ), std::cos( coneAngleAzimuth ), 0.0 },
{ 0.0, 0.0, 1.0 } };
std::vector< std::vector< double > > yBasicRotationMatrix
{ { std::cos( coneAngleDeclination ), 0.0, -std::sin( coneAngleDeclination ) },
{ 0.0, 1.0, 0.0 },
{ std::sin( coneAngleDeclination ), 0.0, std::cos( coneAngleDeclination ) } };
std::vector< std::vector< double > > nonTransposedRotationMatrix( 3, std::vector< double > ( 3 ) );
matrixMultiplication( yBasicRotationMatrix,
zBasicRotationMatrix,
nonTransposedRotationMatrix,
3,
3,
3,
3 );
std::vector< std::vector< double > > intermediateFrameRotationMatrix( 3, std::vector< double > ( 3 ) );
matrixTranspose( nonTransposedRotationMatrix, intermediateFrameRotationMatrix );
std::vector< std::vector< double > > rotatedIntermediateFrameVelocityVector
{ { regolithVelocityVector[ 0 ] },
{ regolithVelocityVector[ 1 ] },
{ regolithVelocityVector[ 2 ] } };
std::vector< std::vector< double > > intermediateFrameVelocityVector( 3, std::vector< double > ( 1 ) );
matrixMultiplication( intermediateFrameRotationMatrix,
rotatedIntermediateFrameVelocityVector,
intermediateFrameVelocityVector,
3,
3,
3,
1 );
// now obtain the basis vectors for the intermediate frame expressed in the
// body fixed frame coordinates
std::vector< double > xUnitVector( 3 );
std::vector< double > yUnitVector( 3 );
std::vector< double > zUnitVector( 3 );
zUnitVector = unitNormalVector;
std::vector< double > bodyFrameZUnitVector { 0.0, 0.0, 1.0 };
// get the intermediate RTN frame at the surface point
std::vector< double > unitR = normalize( regolithPositionVector );
std::vector< double > unitT = normalize( crossProduct( unitR, bodyFrameZUnitVector ) );
// get the x basis vector, pointing to north
xUnitVector = normalize( crossProduct( unitT, zUnitVector ) );
// get the y basis vector
yUnitVector = normalize( crossProduct( zUnitVector, xUnitVector ) );
std::vector< double > zPrincipalAxisBodyFrame { 0.0, 0.0, 1.0 };
std::vector< double > zNegativePrincipalAxisBodyFrame { 0.0, 0.0, -1.0 };
// check if the position vector is along the poles
const double positionDotPrincipalZ
= dotProduct( normalize( regolithPositionVector ), zPrincipalAxisBodyFrame );
const double positionDotNegativePrincipalZ
= dotProduct( normalize( regolithPositionVector ), zNegativePrincipalAxisBodyFrame );
if( positionDotPrincipalZ == 1.0 )
{
// the position vector is pointing to the poles, hence x basis vector pointing to the north
// direction wouldn't work
xUnitVector = { 1.0, 0.0, 0.0 };
yUnitVector = { 0.0, 1.0, 0.0 };
}
else if( positionDotNegativePrincipalZ == 1.0 )
{
xUnitVector = { -1.0, 0.0, 0.0 };
yUnitVector = { 0.0, 1.0, 0.0 };
}
// put the basis vectors in a 3x3 matrix
std::vector< std::vector< double > > intermediateFrameBasisMatrix
{ { xUnitVector[ 0 ], yUnitVector[ 0 ], zUnitVector[ 0 ] },
{ xUnitVector[ 1 ], yUnitVector[ 1 ], zUnitVector[ 1 ] },
{ xUnitVector[ 2 ], yUnitVector[ 2 ], zUnitVector[ 2 ] } };
std::vector< std::vector< double > > bodyFrameVelocityVector( 3, std::vector< double >( 1 ) );
matrixMultiplication( intermediateFrameBasisMatrix,
intermediateFrameVelocityVector,
bodyFrameVelocityVector,
3,
3,
3,
1 );
// return the final regolith velocity vector, expressed in body frame coordinates
regolithVelocityVector[ 0 ] = bodyFrameVelocityVector[ 0 ][ 0 ];
regolithVelocityVector[ 1 ] = bodyFrameVelocityVector[ 1 ][ 0 ];
regolithVelocityVector[ 2 ] = bodyFrameVelocityVector[ 2 ][ 0 ];
}
// Input: M (3x3 mtx)
// Output: Q (3x3 rotation mtx), S (3x3 symmetric mtx)
double PolarDecomposition::Compute(const double * M, double * Q, double * S, double tolerance)
{
double Mk[9];
double Ek[9];
double det, M_oneNorm, M_infNorm, E_oneNorm;
// Mk = M^T
for(int i=0; i<3; i++)
for(int j=0; j<3; j++)
Mk[3 * i + j] = M[3 * j + i];
M_oneNorm = oneNorm(Mk);
M_infNorm = infNorm(Mk);
do
{
double MadjTk[9];
// row 2 x row 3
crossProduct(&(Mk[3]), &(Mk[6]), &(MadjTk[0]));
// row 3 x row 1
crossProduct(&(Mk[6]), &(Mk[0]), &(MadjTk[3]));
// row 1 x row 2
crossProduct(&(Mk[0]), &(Mk[3]), &(MadjTk[6]));
det = Mk[0] * MadjTk[0] + Mk[1] * MadjTk[1] + Mk[2] * MadjTk[2];
if (det == 0.0)
{
printf("Warning (polarDecomposition) : zero determinant encountered.\n");
break;
}
double MadjT_one = oneNorm(MadjTk);
double MadjT_inf = infNorm(MadjTk);
double gamma = sqrt(sqrt((MadjT_one * MadjT_inf) / (M_oneNorm * M_infNorm)) / fabs(det));
double g1 = gamma * 0.5;
double g2 = 0.5 / (gamma * det);
for(int i=0; i<9; i++)
{
Ek[i] = Mk[i];
Mk[i] = g1 * Mk[i] + g2 * MadjTk[i];
Ek[i] -= Mk[i];
}
E_oneNorm = oneNorm(Ek);
M_oneNorm = oneNorm(Mk);
M_infNorm = infNorm(Mk);
}
while ( E_oneNorm > M_oneNorm * tolerance );
// Q = Mk^T
for(int i=0; i<3; i++)
for(int j=0; j<3; j++)
Q[3*i+j] = Mk[3*j+i];
for(int i=0; i<3; i++)
for(int j=0; j<3; j++)
{
S[3*i+j] = 0;
for(int k=0; k<3; k++)
S[3*i+j] += Mk[3*i+k] * M[3*k+j];
}
// S must be symmetric; enforce the symmetry
for (int i=0; i<3; i++)
for (int j=i; j<3; j++)
S[3 * i + j] = S[3 * j + i] = 0.5 * (S[3 * i + j] + S[3 * j + i]);
return (det);
}
float Math::area(FloatVector3d v1,FloatVector3d v2, FloatVector3d v3){
float f=0.5*crossProduct( v2-v1, v3-v1).magnitude();
return f;
}
void computeRegolithVelocityVector2( std::vector< double > regolithPositionVector,
const double velocityMagnitude,
const double coneAngleAzimuth,
const double coneAngleDeclination,
std::vector< double > &unitNormalVector,
std::vector< double > ®olithVelocityVector )
{
// get the z basis vector of the surface frame
std::vector< double > zUnitVector = unitNormalVector;
std::vector< double > xUnitVector( 3 );
std::vector< double > yUnitVector( 3 );
// body frame principal axis Z
std::vector< double > zPrincipalAxisBodyFrame { 0.0, 0.0, 1.0 };
std::vector< double > zNegativePrincipalAxisBodyFrame { 0.0, 0.0, -1.0 };
// check if the position vector is along the poles
const double positionDotPrincipalZ
= dotProduct( normalize( regolithPositionVector ), zPrincipalAxisBodyFrame );
const double positionDotNegativePrincipalZ
= dotProduct( normalize( regolithPositionVector ), zNegativePrincipalAxisBodyFrame );
if( positionDotPrincipalZ == 1.0 )
{
// the position vector is pointing to the poles, hence x basis vector pointing to the north
// direction wouldn't work
xUnitVector = { 1.0, 0.0, 0.0 };
yUnitVector = { 0.0, 1.0, 0.0 };
}
else if( positionDotNegativePrincipalZ == 1.0 )
{
xUnitVector = { -1.0, 0.0, 0.0 };
yUnitVector = { 0.0, 1.0, 0.0 };
}
else
{
// do the regular thing where the x basis is pointing to north
// get the intermediate RTN frame at the surface point
std::vector< double > unitR = normalize( regolithPositionVector );
std::vector< double > unitT = normalize( crossProduct( unitR, zPrincipalAxisBodyFrame ) );
// get the x basis vector, pointing to north
xUnitVector = normalize( crossProduct( unitT, zUnitVector ) );
// get the y basis vector
yUnitVector = normalize( crossProduct( zUnitVector, xUnitVector ) );
}
const double cosDelta = std::cos( coneAngleDeclination );
const double sinDelta = std::sin( coneAngleDeclination );
const double cosGamma = std::cos( coneAngleAzimuth );
const double sinGamma = std::sin( coneAngleAzimuth );
regolithVelocityVector[ 0 ] = velocityMagnitude * ( cosDelta * zUnitVector[ 0 ]
+ sinDelta * cosGamma * xUnitVector[ 0 ]
+ sinDelta * sinGamma * yUnitVector[ 0 ] );
regolithVelocityVector[ 1 ] = velocityMagnitude * ( cosDelta * zUnitVector[ 1 ]
+ sinDelta * cosGamma * xUnitVector[ 1 ]
+ sinDelta * sinGamma * yUnitVector[ 1 ] );
regolithVelocityVector[ 2 ] = velocityMagnitude * ( cosDelta * zUnitVector[ 2 ]
+ sinDelta * cosGamma * xUnitVector[ 2 ]
+ sinDelta * sinGamma * yUnitVector[ 2 ] );
}
void Gizmo::renderQuarterRing(Pipeline& pipeline, const Matrix& mtx, const Vec3& a, const Vec3& b, uint32 color)
{
Vertex vertices[1200];
uint16 indices[1200];
const float ANGLE_STEP = Math::degreesToRadians(1.0f / 100.0f * 360.0f);
Vec3 n = crossProduct(a, b) * 0.05f;
int offset = -1;
for (int i = 0; i < 25; ++i)
{
float angle = i * ANGLE_STEP;
float s = sinf(angle);
float c = cosf(angle);
float sn = sinf(angle + ANGLE_STEP);
float cn = cosf(angle + ANGLE_STEP);
Vec3 p0 = a * s + b * c - n * 0.5f;
Vec3 p1 = a * sn + b * cn - n * 0.5f;
++offset;
vertices[offset].position = p0;
vertices[offset].color = color;
indices[offset] = offset;
++offset;
vertices[offset].position = p1;
vertices[offset].color = color;
indices[offset] = offset;
++offset;
vertices[offset].position = p0 + n;
vertices[offset].color = color;
indices[offset] = offset;
++offset;
vertices[offset].position = p1;
vertices[offset].color = color;
indices[offset] = offset;
++offset;
vertices[offset].position = p1 + n;
vertices[offset].color = color;
indices[offset] = offset;
++offset;
vertices[offset].position = p0 + n;
vertices[offset].color = color;
indices[offset] = offset;
}
auto& renderer = static_cast<Lumix::Renderer&>(m_scene->getPlugin());
Lumix::TransientGeometry ring_geom(vertices, offset, renderer.getBasicVertexDecl(), indices, offset);
pipeline.render(ring_geom,
mtx,
0,
offset,
BGFX_STATE_DEPTH_TEST_LEQUAL,
m_shader->getInstance(0).m_program_handles[pipeline.getPassIdx()]);
const int GRID_SIZE = 5;
offset = -1;
for (int i = 0; i <= GRID_SIZE; ++i)
{
float t = 1.0f / GRID_SIZE * i;
float ratio = sinf(acosf(t));
++offset;
vertices[offset].position = a * t;
vertices[offset].color = color;
indices[offset] = offset;
++offset;
vertices[offset].position = a * t + b * ratio;
vertices[offset].color = color;
indices[offset] = offset;
++offset;
vertices[offset].position = b * t + a * ratio;
vertices[offset].color = color;
indices[offset] = offset;
++offset;
vertices[offset].position = b * t;
vertices[offset].color = color;
indices[offset] = offset;
}
Lumix::TransientGeometry plane_geom(vertices, offset, renderer.getBasicVertexDecl(), indices, offset);
pipeline.render(plane_geom,
mtx,
0,
offset,
BGFX_STATE_DEPTH_TEST_LEQUAL | BGFX_STATE_PT_LINES,
m_shader->getInstance(0).m_program_handles[pipeline.getPassIdx()]);
};
/**
* Construction from a cube (axes aligned) and triangle
*/
CubeTriangleIsect::CubeTriangleIsect(int64_t cube[2][3], int64_t tri[3][3], int64_t error, int triind)
{
int i;
inherit = new TriangleProjection;
inherit->index = triind;
int64_t axes[NUM_AXES][3];
create_projection_axes(axes, tri);
/* Normalize face normal and store */
double dedge1[] = {(double)tri[1][0] - (double)tri[0][0],
(double)tri[1][1] - (double)tri[0][1],
(double)tri[1][2] - (double)tri[0][2]};
double dedge2[] = {(double)tri[2][0] - (double)tri[1][0],
(double)tri[2][1] - (double)tri[1][1],
(double)tri[2][2] - (double)tri[1][2]};
crossProduct(inherit->norm, dedge1, dedge2);
normalize(inherit->norm);
int64_t cubeedge[3][3];
for (i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
cubeedge[i][j] = 0;
}
cubeedge[i][i] = cube[1][i] - cube[0][i];
}
/* Project the cube on to each axis */
for (int axis = 0; axis < NUM_AXES; axis++) {
CubeProjection &cube_proj = cubeProj[axis];
/* Origin */
cube_proj.origin = dotProduct(axes[axis], cube[0]);
/* 3 direction vectors */
for (i = 0; i < 3; i++)
cube_proj.edges[i] = dotProduct(axes[axis], cubeedge[i]);
/* Offsets of 2 ends of cube projection */
int64_t max = 0;
int64_t min = 0;
for (i = 1; i < 8; i++) {
int64_t proj = (vertmap[i][0] * cube_proj.edges[0] +
vertmap[i][1] * cube_proj.edges[1] +
vertmap[i][2] * cube_proj.edges[2]);
if (proj > max) {
max = proj;
}
if (proj < min) {
min = proj;
}
}
cube_proj.min = min;
cube_proj.max = max;
}
/* Project the triangle on to each axis */
for (int axis = 0; axis < NUM_AXES; axis++) {
const int64_t vts[3] = {dotProduct(axes[axis], tri[0]),
dotProduct(axes[axis], tri[1]),
dotProduct(axes[axis], tri[2])};
// Triangle
inherit->tri_proj[axis][0] = vts[0];
inherit->tri_proj[axis][1] = vts[0];
for (i = 1; i < 3; i++) {
if (vts[i] < inherit->tri_proj[axis][0])
inherit->tri_proj[axis][0] = vts[i];
if (vts[i] > inherit->tri_proj[axis][1])
inherit->tri_proj[axis][1] = vts[i];
}
}
}
void ERMSD::calcMat(const std::vector<Vector> & positions,const Pbc& pbc, std::vector<Vector4d> &mat, std::vector<TensorGeneric<4,3> > &Gderi) {
std::vector<Vector3d> pos;
pos.resize(3*nresidues);
std::vector<Tensor3d> deri;
deri.resize(nresidues*9);
std::vector<Vector> centers;
centers.resize(nresidues);
unsigned idx_deri = 0;
Tensor da_dxa = (2./3.)*Tensor::identity();
Tensor da_dxb = -(1./3.)*Tensor::identity();
Tensor da_dxc = -(1./3.)*Tensor::identity();
Tensor db_dxa = -(1./3.)*Tensor::identity();
Tensor db_dxb = (2./3.)*Tensor::identity();
Tensor db_dxc = -(1./3.)*Tensor::identity();
// Form factors - should this be somewhere else?
double w = 1./3.;
Vector form_factor = Vector(2.0,2.0,1.0/0.3);
for(unsigned res_idx=0; res_idx<natoms/3; res_idx++) {
const unsigned at_idx = 3*res_idx;
//center
for (unsigned j=0; j<3; j++) {
centers[res_idx] += w*positions[at_idx+j];
}
Vector3d a = delta(centers[res_idx],positions[at_idx]);
Vector3d b = delta(centers[res_idx],positions[at_idx+1]);
Vector3d d = crossProduct(a,b);
double ianorm = 1./a.modulo();
double idnorm = 1./d.modulo();
// X vector: COM-C2
pos[at_idx] = a*ianorm;
// Z versor: C2 x (COM-C4/C6)
pos[at_idx+2] = d*idnorm;
// Y versor: Z x Y
pos[at_idx+1] = crossProduct(pos[at_idx+2],pos[at_idx]);
// Derivatives ////////
Tensor3d t1 = ianorm*(Tensor::identity()-extProduct(pos[at_idx],pos[at_idx]));
// dv1/dxa
deri[idx_deri] = (2./3. )*t1;
// dv1/dxb
deri[idx_deri+3] = -(1./3.)*t1;
// dv1/dxc
deri[idx_deri+6] = -(1./3.)*t1;
Tensor dd_dxa = VcrossTensor(a,db_dxa) -VcrossTensor(b,da_dxa);
Tensor dd_dxb = VcrossTensor(a,db_dxb)-VcrossTensor(b,da_dxb);
Tensor dd_dxc = VcrossTensor(a,db_dxc)-VcrossTensor(b,da_dxc);
// dv3/dxa
deri[idx_deri+2] = deriNorm(d,dd_dxa);
// dv3/dxb
deri[idx_deri+5] = deriNorm(d,dd_dxb);
// dv3/dxc
deri[idx_deri+8] = deriNorm(d,dd_dxc);
// dv2/dxa = dv3/dxa cross v1 + v3 cross dv1/dxa
deri[idx_deri+1] = (VcrossTensor(deri[idx_deri+2],pos[at_idx]) + \
VcrossTensor(pos[at_idx+2],deri[idx_deri]));
// dv2/dxb
deri[idx_deri+4] = (VcrossTensor(deri[idx_deri+5],pos[at_idx]) + \
VcrossTensor(pos[at_idx+2],deri[idx_deri+3]));
// dv2/dxc
deri[idx_deri+7] = (VcrossTensor(deri[idx_deri+8],pos[at_idx]) + \
VcrossTensor(pos[at_idx+2],deri[idx_deri+6]));
idx_deri += 9;
// End derivatives ///////
}
// Initialization (unnecessary?)
for (unsigned i1=0; i1<nresidues*nresidues; i1++) {
for (unsigned i2=0; i2<4; i2++) {
mat[i1][i2] = 0.0;
}
}
double maxdist = cutoff/form_factor[0];
double gamma = pi/cutoff;
unsigned idx;
unsigned idx1 = 0;
// Calculate mat
for (unsigned i=0; i<nresidues; i++) {
for (unsigned j=0; j<nresidues; j++) {
// skip i==j
if(inPair(i,j) and i != j) {
//if(i!=j){
// Calculate normal distance first
Vector diff = delta(centers[i],centers[j]);
double d1 = diff.modulo();
//std::cout << inPair(i,j) << " " << i << " " << j << " "<< d1 <<"\n";
//std::cout << inPair(i,j) << " " << i << " " << j << " "<< d1 <<"\n";
if(d1<maxdist) {
// calculate r_tilde_ij
Vector3d rtilde;
for (unsigned k=0; k<3; k++) {
for (unsigned l=0; l<3; l++) {
rtilde[l] += pos[3*i+l][k]*diff[k]*form_factor[l];
}
}
double rtilde_norm = rtilde.modulo();
double irnorm = 1./rtilde_norm;
// ellipsoidal cutoff
if(rtilde_norm < cutoff) {
idx = i*nresidues + j;
//std::cout << i << " " << j << " " << rtilde_norm << " " << idx <<"\n";
// fill 4d matrix
double dummy = sin(gamma*rtilde_norm)/(rtilde_norm*gamma);
mat[idx][0] = dummy*rtilde[0];
mat[idx][1] = dummy*rtilde[1];
mat[idx][2] = dummy*rtilde[2];
mat[idx][3] = (1.+ cos(gamma*rtilde_norm))/gamma;
// Derivative (drtilde_dx)
std::vector<Tensor3d> drtilde_dx;
drtilde_dx.resize(6);
unsigned pos_idx = 3*i;
unsigned deri_idx = 9*i;
for (unsigned at=0; at<3; at++) {
for (unsigned l=0; l<3; l++) {
Vector3d rvec = form_factor[l]*((pos[pos_idx+l])/3.);
Vector3d vvec = form_factor[l]*(matmul(deri[deri_idx+3*at+l],diff));
drtilde_dx[at].setRow(l,vvec-rvec);
drtilde_dx[at+3].setRow(l,rvec);
}
}
//std::vector<TensorGeneric<4,3> > dG_dx;
//dG_dx.resize(6);
double dummy1 = (cos(gamma*rtilde_norm) - dummy);
idx1 = i*nresidues*6 + j*6;
for (unsigned l=0; l<6; l++) {
//std::cout << i << " " << j << " " << idx1 << " " << idx1+l << "\n";
// components 1,2,3
// sin(gamma*|rtilde|)/gamma*|rtilde|*d_rtilde +
// + ((d_rtilde*r_tilde/r_tilde^2) out r_tilde)*
// (cos(gamma*|rtilde| - sin(gamma*|rtilde|)/gamma*|rtilde|))
Vector3d rdr = matmul(rtilde,drtilde_dx[l]);
Tensor tt = dummy*drtilde_dx[l] + (dummy1*irnorm*irnorm)*Tensor(rtilde,rdr);
for (unsigned m=0; m<3; m++) {
// Transpose here
//dG_dx[l].setRow(m,tt.getRow(m));
Gderi[idx1+l].setRow(m,tt.getRow(m));
}
// component 4
// - sin(gamma*|rtilde|)/|rtilde|*(r_tilde*d_rtilde)
//dG_dx[l].setRow(3,-dummy*gamma*rdr);
Gderi[idx1+l].setRow(3,-dummy*gamma*rdr);
}
}
}
}
}
}
}
/* implement touch ground pillar ( 4 faces ) to support roller coaster */
void drawSupportedPillar()
{
/* horizontal scale factor */
float cross_step_size=0.08;
/* vertical scale factor */
float up_step_size=0.04;
/* tube scale factor */
float tube_step_size=0.006;
/* parallel rail number (implement V shape so we have 3 parallel spline path and therefore for loop 3 times ) */
for(int index=1;index<4;index++){
glBegin(GL_QUADS);
/* blue */
glColor3f(0.1,0.1, 0.8);
for (int spline_index = 0; spline_index < g_iNumOfSplines; spline_index++) {
for(int i=-2;i<g_Splines[spline_index].numControlPoints-1;i++) {
for(float u=0.0;u<1.0;u+=0.4) {
/*Point*/
p.x= catmullRomSpline(u, g_Splines[spline_index].points[i].x, g_Splines[spline_index].points[i+1].x, g_Splines[spline_index].points[i+2].x, g_Splines[spline_index].points[i+3].x);
p.y= catmullRomSpline(u, g_Splines[spline_index].points[i].y, g_Splines[spline_index].points[i+1].y, g_Splines[spline_index].points[i+2].y, g_Splines[spline_index].points[i+3].y);
p.z= catmullRomSpline(u, g_Splines[spline_index].points[i].z, g_Splines[spline_index].points[i+1].z, g_Splines[spline_index].points[i+2].z, g_Splines[spline_index].points[i+3].z);
/*Tangent*/
t.x= tangent(u, g_Splines[spline_index].points[i].x, g_Splines[spline_index].points[i+1].x, g_Splines[spline_index].points[i+2].x, g_Splines[spline_index].points[i+3].x);
t.y= tangent(u, g_Splines[spline_index].points[i].y, g_Splines[spline_index].points[i+1].y, g_Splines[spline_index].points[i+2].y, g_Splines[spline_index].points[i+3].y);
t.z= tangent(u, g_Splines[spline_index].points[i].z, g_Splines[spline_index].points[i+1].z, g_Splines[spline_index].points[i+2].z, g_Splines[spline_index].points[i+3].z);
t = unit(t);
/*Normal*/
n = crossProduct(t, n);
n = unit(n);
/*Binormal*/
b = crossProduct(t, n);
b = unit(b);
/* use u+0.02 to generate dense square cross-section
PILAR(SQUARE TUBE) UNDER V-SHAPED SQUARE TUBE RAIL
(P1)v2 _____ v1(P1) (P1)v2 _____ v1(P1)
/ /| / /|
(P1)v3/|___/_| v0(P1) (P1)v3/|___/_| v0(P1)
/ / / / / / / /
/_/__/ / /_/__/ /
(P0)v2|/ | /v1(P0) (P0)v2|/ | /v1(P0)
|____|/ |____|/
(P0)v3 v0(P0) (P0)v3 v0(P0)
p1 v3__ p1 v0 (P1)v2 _____ v1(P1)
/_/| / /| p1 v3__ p1 v0
p0 v3 ||||p0 v0 (P1)v3/|___/_| v0(P1 /_/|
PILAR SUPPORT--> | || / / / / p0 v3 ||||p0 v0
|||| /_/__/ / | || <--- PILAR SUPPORT
p1 v3 ground|.|| p1 v0 ground(P0)v2|/ | /v1(P0) ||||
|||/ |____|/ p1 v3 ground|.|| p1 v0 ground
p0 v3 ground p0 v0 ground (P0)v3 v0(P0) |||/
p0 v3 ground p0 v0 ground
p1 v3__ p1 v0
/_/|
p0 v3 ||||p0 v0
| || <--- PILAR SUPPORT
||||
p1 v3 ground|.|| p1 v0 ground
|||/
p0 v3 ground p0 v0 ground
PILAR SUPPORT right face: p0 v0 - p1 v0 - p1 v0 ground - p0 v0 ground
PILAR SUPPORT back face: p0 v0 - p0 v3 - p0 v3 ground - p0 v0 ground
PILAR SUPPORT left face: p0 v3 - p1 v3 - p1 v3 ground - p0 v3 ground
PILAR SUPPORT front face: p1 v0 - p1 v3 - p1 v3 ground - p1 v0 ground
*/
/*Point*/
p1.x= catmullRomSpline(u+0.02, g_Splines[spline_index].points[i].x, g_Splines[spline_index].points[i+1].x, g_Splines[spline_index].points[i+2].x, g_Splines[spline_index].points[i+3].x);
p1.y= catmullRomSpline(u+0.02, g_Splines[spline_index].points[i].y, g_Splines[spline_index].points[i+1].y, g_Splines[spline_index].points[i+2].y, g_Splines[spline_index].points[i+3].y);
p1.z= catmullRomSpline(u+0.02, g_Splines[spline_index].points[i].z, g_Splines[spline_index].points[i+1].z, g_Splines[spline_index].points[i+2].z, g_Splines[spline_index].points[i+3].z);
/*Tangent*/
t1.x= tangent(u+0.02, g_Splines[spline_index].points[i].x, g_Splines[spline_index].points[i+1].x, g_Splines[spline_index].points[i+2].x, g_Splines[spline_index].points[i+3].x);
t1.y= tangent(u+0.02, g_Splines[spline_index].points[i].y, g_Splines[spline_index].points[i+1].y, g_Splines[spline_index].points[i+2].y, g_Splines[spline_index].points[i+3].y);
t1.z= tangent(u+0.02, g_Splines[spline_index].points[i].z, g_Splines[spline_index].points[i+1].z, g_Splines[spline_index].points[i+2].z, g_Splines[spline_index].points[i+3].z);
t1 = unit(t1);
/*Normal*/
n1 = crossProduct(t1, arbitrary_point);
n1 = unit(n1);
/*Binormal*/
b1 = crossProduct(t1, n1);
b1 = unit(b1);
/* implement V shape roller coaster rail */
/* main bottom rail - Center (V bottom vetex)*/
if(index==1)
{
p.x+=cross_step_size*n.x/2;
p.y+=cross_step_size*n.y/2;
p.z+=cross_step_size*n.z/2;
p1.x+=cross_step_size*n1.x/2;
p1.y+=cross_step_size*n1.y/2;
p1.z+=cross_step_size*n1.z/2;
}
/* side rail - Left-Top(V left vetex)*/
if(index==2)
{
p.x+=up_step_size*b.x*up_step_size_Factor;
p.y+=up_step_size*b.y*up_step_size_Factor;
p.z+=up_step_size*b.z*up_step_size_Factor;
p1.x+=up_step_size*b1.x*up_step_size_Factor;
p1.y+=up_step_size*b1.y*up_step_size_Factor;
p1.z+=up_step_size*b1.z*up_step_size_Factor;
}
/* side rail - Right-Top(V right vetex)*/
if(index==3)
{
p.x+=cross_step_size*n.x+ up_step_size*b.x*up_step_size_Factor;
p.y+=cross_step_size*n.y+ up_step_size*b.y*up_step_size_Factor;
p.z+=cross_step_size*n.z+ up_step_size*b.z*up_step_size_Factor;
p1.x+=cross_step_size*n1.x+ up_step_size*b1.x*up_step_size_Factor;
p1.y+=cross_step_size*n1.y+ up_step_size*b1.y*up_step_size_Factor;
p1.z+=cross_step_size*n1.z+ up_step_size*b1.z*up_step_size_Factor;
}
/* implement 4 face of a pillar: right, backawrd, left, forward*/
/* .v2(-n+b) .v1(+n+b)
.v3(-n-b) .v0(+n-b)
*/
/* pillar right face */
/* p0 v0 */
glVertex3f(p.x + tube_step_size*( n.x - b.x), p.y + tube_step_size*( n.y - b.y), p.z + tube_step_size*( n.z - b.z));
/* p1 v0 */
glVertex3f(p1.x + tube_step_size*( n1.x - b1.x), p.y + tube_step_size*( n1.y - b1.y), p1.z + tube_step_size*( n1.z - b1.z));
/* p1 v0 ground */
glVertex3f(p1.x + tube_step_size*( n1.x - b1.x), p.y + tube_step_size*( n1.y - b1.y), -3);
/* p0 v0 ground */
glVertex3f(p.x + tube_step_size*( n.x - b.x), p.y + tube_step_size*( n.y - b.y), -3);
/* pillar back face */
/* p0 v0 */
glVertex3f(p.x + tube_step_size*( n.x - b.x), p.y + tube_step_size*( n.y - b.y), p.z + tube_step_size*( n.z - b.z));
/* p0 v3 */
glVertex3f(p.x + tube_step_size*( -n.x - b.x), p.y + tube_step_size*(- n.y - b.y), p.z + tube_step_size*( -n.z - b.z));
/* p0 v3 ground */
glVertex3f(p.x + tube_step_size*(-n.x - b.x), p.y + tube_step_size*(-n.y - b.y), -3);
/* p0 v0 ground */
glVertex3f(p.x + tube_step_size*(n.x - b.x), p.y + tube_step_size*(n.y - b.y), -3);
/* pillar left face */
/* p0 v3 */
glVertex3f(p.x + tube_step_size*(-n.x - b.x), p.y + tube_step_size*(-n.y - b.y), p.z + tube_step_size*(-n.z - b.z));
/* p1 v3 */
glVertex3f(p1.x + tube_step_size*(-n1.x - b1.x), p1.y + tube_step_size*(-n1.y - b1.y), p1.z + tube_step_size*(-n1.z - b1.z));
/* p1 v3 ground */
glVertex3f(p1.x + tube_step_size*(-n1.x - b1.x), p1.y + tube_step_size*(-n1.y - b1.y), -3);
/* p0 v3 ground */
glVertex3f(p.x + tube_step_size*(-n.x - b.x), p.y + tube_step_size*(-n.y - b.y), -3);
/* pillar front face */
/* p1 v0 */
glVertex3f(p1.x + tube_step_size*( n1.x - b1.x), p1.y + tube_step_size*( n1.y - b1.y), p1.z + tube_step_size*( n1.z - b1.z));
/* p1 v3 */
glVertex3f(p1.x + tube_step_size*(-n1.x - b1.x), p1.y + tube_step_size*(-n1.y - b1.y), p1.z + tube_step_size*(-n1.z - b1.z));
/* p1 v3 ground */
glVertex3f(p1.x + tube_step_size*(-n1.x - b1.x), p1.y + tube_step_size*(-n1.y - b1.y), -3);
/* p1 v0 ground */
glVertex3f(p1.x + tube_step_size*( n1.x - b1.x), p1.y + tube_step_size*( n1.y - b1.y), -3);
}/* end of for loop*/
}/* end of for loop*/
}/* end of for loop*/
glEnd();
}/* end of for loop*/
}
bool isClockwiseOriented(sf::Vector2f v0, sf::Vector2f v1, sf::Vector2f v2)
{
return crossProduct(v1 - v0, v2 - v0).z <= 0;
}
RayCastModelHit Model::castRay(const Vec3& origin,
const Vec3& dir,
const Matrix& model_transform)
{
RayCastModelHit hit;
hit.m_is_hit = false;
if (!isReady())
{
return hit;
}
Matrix inv = model_transform;
inv.inverse();
Vec3 local_origin = inv.multiplyPosition(origin);
Vec3 local_dir = static_cast<Vec3>(inv * Vec4(dir.x, dir.y, dir.z, 0));
const Array<Vec3>& vertices = m_vertices;
const Array<int32_t>& indices = m_indices;
int vertex_offset = 0;
for (int mesh_index = 0; mesh_index < m_meshes.size(); ++mesh_index)
{
int indices_end = m_meshes[mesh_index].getIndicesOffset() +
m_meshes[mesh_index].getIndexCount();
for (int i = m_meshes[mesh_index].getIndicesOffset(); i < indices_end;
i += 3)
{
Vec3 p0 = vertices[vertex_offset + indices[i]];
Vec3 p1 = vertices[vertex_offset + indices[i + 1]];
Vec3 p2 = vertices[vertex_offset + indices[i + 2]];
Vec3 normal = crossProduct(p1 - p0, p2 - p0);
float q = dotProduct(normal, local_dir);
if (q == 0)
{
continue;
}
float d = -dotProduct(normal, p0);
float t = -(dotProduct(normal, local_origin) + d) / q;
if (t < 0)
{
continue;
}
Vec3 hit_point = local_origin + local_dir * t;
Vec3 edge0 = p1 - p0;
Vec3 VP0 = hit_point - p0;
if (dotProduct(normal, crossProduct(edge0, VP0)) < 0)
{
continue;
}
Vec3 edge1 = p2 - p1;
Vec3 VP1 = hit_point - p1;
if (dotProduct(normal, crossProduct(edge1, VP1)) < 0)
{
continue;
}
Vec3 edge2 = p0 - p2;
Vec3 VP2 = hit_point - p2;
if (dotProduct(normal, crossProduct(edge2, VP2)) < 0)
{
continue;
}
if (!hit.m_is_hit || hit.m_t > t)
{
hit.m_is_hit = true;
hit.m_t = t;
hit.m_mesh = &m_meshes[mesh_index];
}
}
vertex_offset += m_meshes[mesh_index].getAttributeArraySize() /
m_meshes[mesh_index].getVertexDefinition().getStride();
}
hit.m_origin = origin;
hit.m_dir = dir;
return hit;
}
void setNormal() {
Vector u(b.x - a.x, b.y - a.y, b.z - a.z);
Vector v(c.x - a.x, c.y - a.y, c.z - a.z);
normal = crossProduct(u, v);
normalize(normal);
}
//This finds a bearing, correcting for board tilt and roll as measured by the accelerometer
//This doesn't account for dynamic acceleration - ie accelerations other then gravity will throw off the calculation
// but we can help by feeding it low-pass filtered data
float compassBearingCompute(compassBearing_t * ptrCompassBearing){
assert(ptrCompassBearing); // Defensive Code: Prevent nulls
float Xh = 0;
float Yh = 0;
//find the tilt of the board wrt gravity
float gravity[4][2] = {};
gravity[1][1] = ptrCompassBearing->ax;
gravity[2][1] = ptrCompassBearing->az;
gravity[3][1] = ptrCompassBearing->ay;
normalize(gravity);
float pitchAngle = asin(gravity[1][1]);
float rollAngle = asin(gravity[3][1]);
float yawAngle = atan2(ptrCompassBearing->cz * sin(rollAngle) - ptrCompassBearing->cy * cos(rollAngle) ,
ptrCompassBearing->cx + cos(pitchAngle) +
ptrCompassBearing->cy * sin(pitchAngle) * sin(rollAngle) +
ptrCompassBearing->cz * sin(pitchAngle) * cos(rollAngle)
);
//The board is up-side down
if (gravity[2][1] < 0)
{
pitchAngle = -pitchAngle;
rollAngle = -rollAngle;
}
//Construct a rotation matrix for rotating vectors measured in the body frame, into the earth frame
//this is done by using the angles between the board and the gravity vector.
float xRotMatrix[4][4] = {};
xRotMatrix[1][1] = cos(pitchAngle); xRotMatrix[2][1] = -sin(pitchAngle); xRotMatrix[3][1] = 0;
xRotMatrix[1][2] = sin(pitchAngle); xRotMatrix[2][2] = cos(pitchAngle); xRotMatrix[3][2] = 0;
xRotMatrix[1][3] = 0; xRotMatrix[2][3] = 0; xRotMatrix[3][3] = 1;
float zRotMatrix[4][4] = {};
zRotMatrix[1][1] = 1; zRotMatrix[2][1] = 0; zRotMatrix[3][1] = 0;
zRotMatrix[1][2] = 0; zRotMatrix[2][2] = cos(rollAngle); zRotMatrix[3][2] = -sin(rollAngle);
zRotMatrix[1][3] = 0; zRotMatrix[2][3] = sin(rollAngle); zRotMatrix[3][3] = cos(rollAngle);
float rotMatrix[4][4] = {};
crossProduct((float *)xRotMatrix, (float *)zRotMatrix, (float *)rotMatrix, 3, 3, 3, 3);
//These represent the x and y components of the magnetic field vector in the earth frame
Xh = -(rotMatrix[1][3] * ptrCompassBearing->cx + rotMatrix[2][3] * ptrCompassBearing->cz + rotMatrix[3][3] * -(ptrCompassBearing->cy));
Yh = -(rotMatrix[1][1] * ptrCompassBearing->cx + rotMatrix[2][1] * ptrCompassBearing->cz + rotMatrix[3][1] * -(ptrCompassBearing->cy));
//we use the computed X-Y to find a magnetic North bearing in the earth frame
float _360inRads = (360 * M_PI / 180.0);
// Note: If we don't initialize to '0.0f' then we get an xcode analyzer
// warning saying: "The left operand of '!=' is a garbage value"
float newBearing = 0.0f;
if (Xh < 0)
newBearing = M_PI - atan(Yh / Xh);
else if (Xh > 0 && Yh < 0)
newBearing = -atan(Yh / Xh);
else if (Xh > 0 && Yh > 0)
newBearing = M_PI * 2 - atan(Yh / Xh);
else if (Xh == 0 && Yh < 0)
newBearing = M_PI / 2.0;
else if (Xh == 0 && Yh > 0)
newBearing = M_PI * 1.5;
//The board is up-side down
if (gravity[2][1] < 0)
{
newBearing = fabs(newBearing - _360inRads);
}
//Add in declination
if(ptrCompassBearing->useDeclination)
{
newBearing = (newBearing + ptrCompassBearing->declination);
if(newBearing > _360inRads)
newBearing -= _360inRads;
if(newBearing < 0)
newBearing += _360inRads;
}
if (fabs(newBearing - ptrCompassBearing->bearing) > 2) //2 radians == ~115 degrees
{
if(newBearing > ptrCompassBearing->bearing)
ptrCompassBearing->bearing += _360inRads;
else
ptrCompassBearing->bearing -= _360inRads;
}
ptrCompassBearing->bearing = newBearing * ptrCompassBearing->filterConstant + ptrCompassBearing->bearing * (1.0 - ptrCompassBearing->filterConstant);
ptrCompassBearing->bearingDegrees = ptrCompassBearing->bearing * (180.0 / M_PI);
ptrCompassBearing->pitchAngle = pitchAngle;
ptrCompassBearing->rollAngle = rollAngle;
ptrCompassBearing->yawAngle = yawAngle;
return ptrCompassBearing->bearingDegrees;
}
// 判断两个矢量是否平行
bool Vector2d::isParallelTo(const Vector2d& vec, const Tol& tol) const
{
float cosfz = dotProduct(vec);
float sinfz = crossProduct(vec);
return (fabs(sinfz) <= fabs(cosfz) * tol.equalVector());
}
// -------------------------------------------------------------------------- //
int Class_VolTri::ReturnSimplexID(
a3vector1D &P,
int S0
) {
// ========================================================================== //
// int Class_VolTri::ReturnSimplexID( //
// a3vector1D &P, //
// int S0) //
// //
// Return the ID of the simplex enclosing point P. //
// ========================================================================== //
// INPUT //
// ========================================================================== //
// - P : a3vector1D, point coordinates //
// - S0 : int, seed for search procedure //
// ========================================================================== //
// OUTPUT //
// ========================================================================== //
// - S : int, ID of simplex enclosing point P //
// ========================================================================== //
// ========================================================================== //
// VARIABLES DECLARATION //
// ========================================================================== //
// Local variables
bool check = false;
int n, m, p;
double max_dp, dp;
bvector1D visited(nSimplex, false);
ivector2D face_vlist;
a3vector1D x, y, xF, xS, dir;
a3vector2D face_normals;
bitpit::LIFOStack<int> stack;
// Counters
int i, j, k, l, S, A;
/*debug*/int n_visited = 0;
// /*debug*/ofstream log_file;
// /*debug*/log_file.open("SEARCH.log", ifstream::app);
// ========================================================================== //
// INITIALIZE PARAMETERS //
// ========================================================================== //
// x.fill(0.0);
// y.fill(0.0);
// /*debug*/log_file << "point coordinates: " << P << endl;
// ========================================================================== //
// BUILD ADJACENCY IF NOT ALREADY BUILT //
// ========================================================================== //
if ((Adjacency.size() == 0) || (Adjacency.size() < nSimplex)) {
BuildAdjacency();
}
// ========================================================================== //
// LOOP UNTIL SIMPLEX IS FOUND //
// ========================================================================== //
stack.push(S0);
while ((stack.TOPSTK > 0) && (!check)) {
// /*debug*/n_visited++;
// Pop item from stack -------------------------------------------------- //
S = stack.pop();
visited[S] = true;
// /*debug*/log_file << " traversing simplex: " << S << endl;
// /*debug*/log_file << " (visited: " << n_visited << " of " << nSimplex << endl;
// Simplex infos -------------------------------------------------------- //
n = infos[e_type[S]].n_vert;
m = infos[e_type[S]].n_faces;
// Get face vertices ---------------------------------------------------- //
{
face_vlist.resize(m);
for (j = 0; j < m; ++j) {
face_vlist[j] = FaceVertices(S, j);
} //next j
}
// Simplex baricenter --------------------------------------------------- //
{
xS.fill(0.0);
for (j = 0; j < n; ++j) {
for (l = 0; l < 3; ++l) {
xS[l] += Vertex[Simplex[S][j]][l];
} //next l
} //next j
xS = xS/((double) n);
}
// Compute face normals ------------------------------------------------- //
{
face_normals.resize(m);
for (j = 0; j < m; ++j) {
if (face_vlist[j].size() == 2) {
x = Vertex[face_vlist[j][1]] - Vertex[face_vlist[j][0]];
x[2] = 0.0;
y[0] = 0.0;
y[1] = 0.0;
y[2] = 1.0;
face_normals[j] = crossProduct(x, y);
}
else {
x = Vertex[face_vlist[j][2]] - Vertex[face_vlist[j][1]];
y = Vertex[face_vlist[j][1]] - Vertex[face_vlist[j][0]];
face_normals[j] = crossProduct(x, y);
}
face_normals[j] = face_normals[j]/max(norm2(face_normals[j]), 2.0e-16);
} //next j
}
// Check if Simplex S encloses point P ---------------------------------- //
{
check = true;
for (j = 0; j < m; ++j) {
n = face_vlist[j].size();
// Compute face center
xF.fill(0.0);
for (k = 0; k < n; ++k) {
for (l = 0; l < 3; l++) {
xF[l] += Vertex[face_vlist[j][k]][l];
} //next l
} //next k
xF = xF/((double) n);
// Check if simplex encloses point
dir = P - xF;
dir = dir/max(norm2(dir), 2.0e-16);
check = (check && (dotProduct(dir, face_normals[j]) <= 0.0));
} //next j
}
// /*debug*/log_file << " encloses point: " << check << endl;
// Look for best direction (Euristic search of best path) --------------- //
// NOTES: //
// - modificare il criterio euristico per la scelta del path migliore //
// * congiungente P-xS attraversa una faccia. //
// * distanza minima dalle facce. //
// ---------------------------------------------------------------------- //
if (!check) {
// local direction of searching path
dir = P - xS;
dir = dir/max(norm2(dir), 2.0e-16);
// Loop over simplex faces
max_dp = -2.0;
i = 0;
j = -1;
while (i < m) {
dp = dotProduct(face_normals[i], dir);
if (dp > max_dp) {
j = i;
max_dp = dp;
}
i++;
} //next i
// Alternative directions
for (i = 0; i < m; ++i) {
A = Adjacency[S][i];
if ((i != j) && (A >= 0) && (!visited[A])) {
stack.push(A);
}
} //next i
A = Adjacency[S][j];
if ((A >= 0) && (!visited[A])) {
stack.push(A);
}
}
} //next simplex
// /*debug*/log_file << " (visited: " << n_visited << " of " << nSimplex << ")" << endl;
// Old algorithm ============================================================ //
{
// while ((n_visited <= nSimplex) && (!check) && (S0 >= 0)) {
// // Update simplex ID ---------------------------------------------------- //
// // cout << " on simplex " << S0;
// S = S0;
// n_visited++;
// visited[S] = true;
// /*debug*/log_file << " traversing simplex: " << S << endl;
// /*debug*/log_file << " (visited: " << n_visited << " of " << nSimplex << endl;
// // Simplex infos -------------------------------------------------------- //
// m = infos[e_type[S]].n_faces;
// p = infos[e_type[S]].n_vert;
// // Compute simplex baricenter ------------------------------------------- //
// {
// xS.fill(0.0);
// for (j = 0; j < p; ++j) {
// for (l = 0; l < dim; ++l) {
// xS[l] += Vertex[Simplex[S][j]][l];
// } //next l
// } //next j
// xS = xS/((double) p);
// }
// // Get face vertices ---------------------------------------------------- //
// {
// face_vlist.resize(m);
// for (j = 0; j < m; ++j) {
// face_vlist[j] = FaceVertices(S, j);
// } //next j
// }
// // Compute face normals ------------------------------------------------- //
// {
// face_normals.resize(m);
// for (j = 0; j < m; ++j) {
// if (face_vlist[j].size() == 2) {
// x[0] = Vertex[face_vlist[j][1]][0] - Vertex[face_vlist[j][0]][0];
// x[1] = Vertex[face_vlist[j][1]][1] - Vertex[face_vlist[j][0]][1];
// x[2] = 0.0;
// y[0] = 0.0;
// y[1] = 0.0;
// y[2] = 1.0;
// face_normals[j] = crossProduct(x, y);
// }
// else {
// x[0] = Vertex[face_vlist[j][2]][0] - Vertex[face_vlist[j][1]][0];
// x[1] = Vertex[face_vlist[j][2]][1] - Vertex[face_vlist[j][1]][1];
// x[2] = Vertex[face_vlist[j][2]][2] - Vertex[face_vlist[j][1]][2];
// y[0] = Vertex[face_vlist[j][1]][0] - Vertex[face_vlist[j][0]][0];
// y[1] = Vertex[face_vlist[j][1]][1] - Vertex[face_vlist[j][0]][1];
// y[2] = Vertex[face_vlist[j][1]][2] - Vertex[face_vlist[j][0]][2];
// face_normals[j] = crossProduct(x, y);
// }
// face_normals[j] = face_normals[j]/max(norm2(face_normals[j]), 2.0e-16);
// } //next j
// }
// // Check if P is enclosed in simplex S0 --------------------------------- //
// {
// check = true;
// for (j = 0; j < m; ++j) {
// n = face_vlist[j].size();
// // Compute face center
// xF.fill(0.0);
// for (k = 0; k < n; ++k) {
// for (l = 0; l < dim; l++) {
// xF[l] += Vertex[face_vlist[j][k]][l];
// } //next l
// } //next k
// xF = xF/((double) n);
// // Check if simplex encloses point
// for (l = 0; l < dim; l++) {
// dir[l] = P[l] - xF[l];
// } //next l
// dir = dir/max(norm2(dir), 2.0e-16);
// check = (check && (dotProduct(dir, face_normals[j]) <= 0.0));
// // cout << " f " << j << ", c: " << check;
// } //next j
// }
// // cout << ", check: " << check << endl;
// /*debug*/log_file << " encloses point: " << check << endl;
// // Look for best direction (Euristic search) ---------------------------- //
// if (!check) {
// // Find face to be crossed (Euristic search of best path)
// {
// // path local direction
// for (l = 0; l < dim; ++l) {
// dir[l] = P[l] - xS[l];
// } //next l
// dir = dir/max(norm2(dir), 2.0e-16);
// // Loop over simplex faces
// // WARNING: infinite loop at corner simplicies!!
// max_dp = -2.0;
// i = -1;
// j = 0;
// while (j < m) {
// dp = dotProduct(face_normals[j], dir);
// A = Adjacency[S][j];
// if ((dp > max_dp) && (A >= 0)) {//&& (!visited[A])) {
// i = j;
// max_dp = dp;
// }
// j++;
// } //next j
// }
// // Move to adjacent simplex
// if (i >= 0) { S0 = Adjacency[S][i]; }
// else { S0 = -1;}
// }
// /*debug*/log_file << " next simplex: " << S0 << endl;
// } //next simplex
}
// /*debug*/log_file.close();
return(S); };
RayCastModelHit Model::castRay(const Vec3& origin, const Vec3& dir, const Matrix& model_transform, const Pose* pose)
{
RayCastModelHit hit;
hit.m_is_hit = false;
if (!isReady()) return hit;
Matrix inv = model_transform;
inv.inverse();
Vec3 local_origin = inv.transformPoint(origin);
Vec3 local_dir = (inv * Vec4(dir.x, dir.y, dir.z, 0)).xyz();
Matrix matrices[256];
ASSERT(!pose || pose->count <= lengthOf(matrices));
bool is_skinned = false;
for (int mesh_index = m_lods[0].from_mesh; mesh_index <= m_lods[0].to_mesh; ++mesh_index)
{
Mesh& mesh = m_meshes[mesh_index];
is_skinned = pose && !mesh.skin.empty() && pose->count <= lengthOf(matrices);
}
if (is_skinned)
{
computeSkinMatrices(*pose, *this, matrices);
}
for (int mesh_index = m_lods[0].from_mesh; mesh_index <= m_lods[0].to_mesh; ++mesh_index)
{
Mesh& mesh = m_meshes[mesh_index];
bool is_mesh_skinned = !mesh.skin.empty();
u16* indices16 = (u16*)&mesh.indices[0];
u32* indices32 = (u32*)&mesh.indices[0];
bool is16 = mesh.flags.isSet(Mesh::Flags::INDICES_16_BIT);
int index_size = is16 ? 2 : 4;
for(int i = 0, c = mesh.indices.size() / index_size; i < c; i += 3)
{
Vec3 p0, p1, p2;
if (is16)
{
p0 = mesh.vertices[indices16[i]];
p1 = mesh.vertices[indices16[i + 1]];
p2 = mesh.vertices[indices16[i + 2]];
if (is_mesh_skinned)
{
p0 = evaluateSkin(p0, mesh.skin[indices16[i]], matrices);
p1 = evaluateSkin(p1, mesh.skin[indices16[i + 1]], matrices);
p2 = evaluateSkin(p2, mesh.skin[indices16[i + 2]], matrices);
}
}
else
{
p0 = mesh.vertices[indices32[i]];
p1 = mesh.vertices[indices32[i + 1]];
p2 = mesh.vertices[indices32[i + 2]];
if (is_mesh_skinned)
{
p0 = evaluateSkin(p0, mesh.skin[indices32[i]], matrices);
p1 = evaluateSkin(p1, mesh.skin[indices32[i + 1]], matrices);
p2 = evaluateSkin(p2, mesh.skin[indices32[i + 2]], matrices);
}
}
Vec3 normal = crossProduct(p1 - p0, p2 - p0);
float q = dotProduct(normal, local_dir);
if (q == 0) continue;
float d = -dotProduct(normal, p0);
float t = -(dotProduct(normal, local_origin) + d) / q;
if (t < 0) continue;
Vec3 hit_point = local_origin + local_dir * t;
Vec3 edge0 = p1 - p0;
Vec3 VP0 = hit_point - p0;
if (dotProduct(normal, crossProduct(edge0, VP0)) < 0) continue;
Vec3 edge1 = p2 - p1;
Vec3 VP1 = hit_point - p1;
if (dotProduct(normal, crossProduct(edge1, VP1)) < 0) continue;
Vec3 edge2 = p0 - p2;
Vec3 VP2 = hit_point - p2;
if (dotProduct(normal, crossProduct(edge2, VP2)) < 0) continue;
if (!hit.m_is_hit || hit.m_t > t)
{
hit.m_is_hit = true;
hit.m_t = t;
hit.m_mesh = &m_meshes[mesh_index];
}
}
}
hit.m_origin = origin;
hit.m_dir = dir;
return hit;
}
void getNormals(GLfloat **hmX, GLfloat **hmY, GLfloat **hmZ, size_t row,size_t col,size_t height,size_t width, GLfloat **sum){
(*sum)= (GLfloat*) malloc(sizeof(GLfloat) * 3);
GLfloat *n;
(*sum)[0]=0;
(*sum)[1]=0;
(*sum)[2]=0;
GLfloat sumX, sumY, sumZ;
sumX=0;
sumY=0;
sumZ=0;
GLfloat v1[3], v2[3];
GLfloat curX = hmX[row][col];
GLfloat curY = hmY[row][col];
GLfloat curZ = hmZ[row][col];
if( row+1 < height && col+1 < width ){
v1[0] = hmX[row+0][col+1] - curX;
v1[1] = hmY[row+0][col+1] - curY;
v1[2] = hmZ[row+0][col+1] - curZ;
v2[0] = hmX[row+1][col+0] - curX;
v2[1] = hmY[row+1][col+0] - curY;
v2[2] = hmZ[row+1][col+0] - curZ;
crossProduct(v1, v2, &n);
normalize(&n);
sumX += n[0];
sumY += n[1];
sumZ += n[2];
}
if( row+1 < height && col > 0 ){
v1[0] = hmX[row+1][col+0] - curX;
v1[1] = hmY[row+1][col+0] - curY;
v1[2] = hmZ[row+1][col+0] - curZ;
v2[0] = hmX[row+1][col-1] - curX;
v2[1] = hmY[row+1][col-1] - curY;
v2[2] = hmZ[row+1][col-1] - curZ;
crossProduct(v1, v2, &n);
normalize(&n);
sumX += n[0];
sumY += n[1];
sumZ += n[2];
}
if( row+1 < height && col > 0 ){
v1[0] = hmX[row+1][col-1] - curX;
v1[1] = hmY[row+1][col-1] - curY;
v1[2] = hmZ[row+1][col-1] - curZ;
v2[0] = hmX[row+0][col-1] - curX;
v2[1] = hmY[row+0][col-1] - curY;
v2[2] = hmZ[row+0][col-1] - curZ;
crossProduct(v1, v2, &n);
normalize(&n);
sumX += n[0];
sumY += n[1];
sumZ += n[2];
}
if( row > 0 && col > 0 ){
v1[0] = hmX[row+0][col-1] - curX;
v1[1] = hmY[row+0][col-1] - curY;
v1[2] = hmZ[row+0][col-1] - curZ;
v2[0] = hmX[row-1][col+0] - curX;
v2[1] = hmY[row-1][col+0] - curY;
v2[2] = hmZ[row-1][col+0] - curZ;
crossProduct(v1, v2, &n);
normalize(&n);
sumX += n[0];
sumY += n[1];
sumZ += n[2];
}
if( row > 0 && col+1 < width ){
v1[0] = hmX[row-1][col+0] - curX;
v1[1] = hmY[row-1][col+0] - curY;
v1[2] = hmZ[row-1][col+0] - curZ;
v2[0] = hmX[row-1][col+1] - curX;
v2[1] = hmY[row-1][col+1] - curY;
v2[2] = hmZ[row-1][col+1] - curZ;
crossProduct(v1, v2, &n);
normalize(&n);
sumX += n[0];
sumY += n[1];
sumZ += n[2];
}
if( row > 0 && col+1 < width ){
v1[0] = hmX[row-1][col+1] - curX;
v1[1] = hmY[row-1][col+1] - curY;
v1[2] = hmZ[row-1][col+1] - curZ;
v2[0] = hmX[row+0][col+1] - curX;
v2[1] = hmY[row+0][col+1] - curY;
v2[2] = hmZ[row+0][col+1] - curZ;
crossProduct(v1, v2, &n);
normalize(&n);
sumX += n[0];
sumY += n[1];
sumZ += n[2];
}
(*sum)[0]= sumX;
(*sum)[1]= sumY;
(*sum)[2]= sumZ;
normalize(sum);
(*sum)[0]= -((*sum)[0]);
(*sum)[1]= -((*sum)[1]);
(*sum)[2]= -((*sum)[2]);
}
TVector TPerturbationBump::perturbNormal (const TSurfaceData& rktDATA) const
{
TVector tNewNormal = rktDATA.unperturbedNormal();
if ( fabs(tBumpFactor) > FX_EPSILON )
{
TSurfaceData tData;
TColor tColor;
TColor tBasisColor;
TVector tGradientU;
TVector tGradientV;
TVector r, s, t;
TVector tTemp;
TScalar tHeight;
TScalar tHeightDiff;
TScalar dx, dy;
TScalar x, y;
TScalar tRDamping;
TScalar tTDamping;
TScalar tRDampingAbs;
TScalar tTDampingAbs;
TScalar tRDampingTotal;
TScalar tTDampingTotal;
TScalar tRTotal;
TScalar tTTotal;
TMatrix tObjectTransform = *rktDATA.object()->transformMatrix();
if ( !ptPattern )
{
cout << ("Error: pattern must be set") << endl;
exit (1);
}
s = tNewNormal;
r = crossProduct (s, TVector(0.0, 1.0, 0.0));
if ( r.norm() < FX_EPSILON )
{
r = TVector(1.0, 0.0, 0.0);
if ( fabs (s.y() - 1.0) < FX_EPSILON )
{
s = TVector(0.0, 1.0, 0.0);
}
else
{
s = TVector(0.0, -1.0, 0.0);
}
}
r.normalize();
t = crossProduct (r, s);
t.normalize();
tGradientU = r * tGradientDisplacement.x();
tGradientV = t * tGradientDisplacement.y();
tData = rktDATA;
tColor = ptPattern->color (rktDATA);
tHeight = tColor.average();
dx = ( tSamples.x() > 1 ) ? (2.0 / (tSamples.x() - 1.0)) : 0;
dy = ( tSamples.y() > 1 ) ? (2.0 / (tSamples.y() - 1.0)) : 0;
tRTotal = 0;
tTTotal = 0;
tRDampingTotal = 0;
tTDampingTotal = 0;
y = -1.0;
for (size_t iy = 0; ( iy < (size_t) tSamples.y() ); iy++)
{
tTDampingAbs = (1.0 - (y * y) * 0.8);
if ( tTDampingAbs > (1.0 - FX_EPSILON) )
{
tTDampingAbs = 0;
}
tTDamping = ( y > 0 ) ? tTDampingAbs : -tTDampingAbs;
x = -1.0;
for (size_t ix = 0; ( ix < (size_t) tSamples.x() ); ix++)
{
tTemp = ptPattern->warp (rktDATA.localPoint()) + (tGradientU * x) + (tGradientV * y);
tData.setPoint (tObjectTransform * tTemp);
tColor = ptPattern->color (tData);
tHeightDiff = tHeight - tColor.average();
tRDampingAbs = (1.0 - (x * x) * 0.8);
if ( tRDampingAbs > (1.0 - FX_EPSILON) )
{
tRDampingAbs = 0;
}
tRDamping = ( x > 0 ) ? tRDampingAbs : -tRDampingAbs;
tRDampingTotal += tRDampingAbs;
tTDampingTotal += tTDampingAbs;
tRTotal += tHeightDiff * tRDamping;
tTTotal += tHeightDiff * tTDamping;
x += dx;
}
y += dy;
}
r *= tRTotal / tRDampingTotal;
t *= tTTotal / tTDampingTotal;
tNewNormal = s + (r + t) * tBumpFactor;
tNewNormal.normalize();
}
return tNewNormal;
} /* perturbNormal() */
double distanceToLine(const Vector3D& pointP, const Vector3D& vecA, const Vector3D& vecB) {
return crossProduct(pointP-vecA, vecB-vecA).Length() / Distance(vecA, vecB);
}
/* . . side rails
. bottom rail
*/
void drawSplines()
{
/* horizontal scale factor */
float cross_step_size=0.08;
/* vertical scale factor */
float up_step_size=0.04;
/* tube scale factor */
float tube_step_size=0.006;
/* parallel rail number (implement V shape so we have 3 parallel spline path and therefore for loop 3 times ) */
for(int index=1;index<4;index++){
glBegin(GL_QUADS);
/* red */
glColor3f(1 , 0, 0);
for (int spline_index = 0; spline_index < g_iNumOfSplines; spline_index++) {
for(int i=-2;i<g_Splines[spline_index].numControlPoints-1;i++) {
for(float u=0.0;u<1.0;u+=0.04) {
/*Point*/
p.x= catmullRomSpline(u, g_Splines[spline_index].points[i].x, g_Splines[spline_index].points[i+1].x, g_Splines[spline_index].points[i+2].x, g_Splines[spline_index].points[i+3].x);
p.y= catmullRomSpline(u, g_Splines[spline_index].points[i].y, g_Splines[spline_index].points[i+1].y, g_Splines[spline_index].points[i+2].y, g_Splines[spline_index].points[i+3].y);
p.z= catmullRomSpline(u, g_Splines[spline_index].points[i].z, g_Splines[spline_index].points[i+1].z, g_Splines[spline_index].points[i+2].z, g_Splines[spline_index].points[i+3].z);
/*Tangent*/
t.x= tangent(u, g_Splines[spline_index].points[i].x, g_Splines[spline_index].points[i+1].x, g_Splines[spline_index].points[i+2].x, g_Splines[spline_index].points[i+3].x);
t.y= tangent(u, g_Splines[spline_index].points[i].y, g_Splines[spline_index].points[i+1].y, g_Splines[spline_index].points[i+2].y, g_Splines[spline_index].points[i+3].y);
t.z= tangent(u, g_Splines[spline_index].points[i].z, g_Splines[spline_index].points[i+1].z, g_Splines[spline_index].points[i+2].z, g_Splines[spline_index].points[i+3].z);
t = unit(t);
/*Normal*/
n = crossProduct(t, arbitrary_point);
n = unit(n);
/*Binormal*/
b = crossProduct(t, n);
b = unit(b);
/* use u+0.02 to generate dense square cross-section */
/*Point*/
p1.x= catmullRomSpline(u+0.04, g_Splines[spline_index].points[i].x, g_Splines[spline_index].points[i+1].x, g_Splines[spline_index].points[i+2].x, g_Splines[spline_index].points[i+3].x);
p1.y= catmullRomSpline(u+0.04, g_Splines[spline_index].points[i].y, g_Splines[spline_index].points[i+1].y, g_Splines[spline_index].points[i+2].y, g_Splines[spline_index].points[i+3].y);
p1.z= catmullRomSpline(u+0.04, g_Splines[spline_index].points[i].z, g_Splines[spline_index].points[i+1].z, g_Splines[spline_index].points[i+2].z, g_Splines[spline_index].points[i+3].z);
/*Tangent*/
t1.x= tangent(u+0.04, g_Splines[spline_index].points[i].x, g_Splines[spline_index].points[i+1].x, g_Splines[spline_index].points[i+2].x, g_Splines[spline_index].points[i+3].x);
t1.y= tangent(u+0.04, g_Splines[spline_index].points[i].y, g_Splines[spline_index].points[i+1].y, g_Splines[spline_index].points[i+2].y, g_Splines[spline_index].points[i+3].y);
t1.z= tangent(u+0.04, g_Splines[spline_index].points[i].z, g_Splines[spline_index].points[i+1].z, g_Splines[spline_index].points[i+2].z, g_Splines[spline_index].points[i+3].z);
t1 = unit(t1);
/*Normal*/
n1 = crossProduct(t1, arbitrary_point);
n1 = unit(n1);
/*Binormal*/
b1 = crossProduct(t1, n1);
b1 = unit(b1);
/* implement V shape roller coaster rail */
/* main bottom rail - Center (V bottom vetex)*/
if(index==1)
{
p.x+=cross_step_size*n.x/2;
p.y+=cross_step_size*n.y/2;
p.z+=cross_step_size*n.z/2;
p1.x+=cross_step_size*n1.x/2;
p1.y+=cross_step_size*n1.y/2;
p1.z+=cross_step_size*n1.z/2;
}
/* side rail - Left-Top(V left vetex)*/
if(index==2)
{
p.x+=up_step_size*b.x*up_step_size_Factor;
p.y+=up_step_size*b.y*up_step_size_Factor;
p.z+=up_step_size*b.z*up_step_size_Factor;
p1.x+=up_step_size*b1.x*up_step_size_Factor;
p1.y+=up_step_size*b1.y*up_step_size_Factor;
p1.z+=up_step_size*b1.z*up_step_size_Factor;
}
/* side rail - Right-Top(V right vetex)*/
if(index==3)
{
p.x+=cross_step_size*n.x+ up_step_size*b.x*up_step_size_Factor;
p.y+=cross_step_size*n.y+ up_step_size*b.y*up_step_size_Factor;
p.z+=cross_step_size*n.z+ up_step_size*b.z*up_step_size_Factor;
p1.x+=cross_step_size*n1.x+ up_step_size*b1.x*up_step_size_Factor;
p1.y+=cross_step_size*n1.y+ up_step_size*b1.y*up_step_size_Factor;
p1.z+=cross_step_size*n1.z+ up_step_size*b1.z*up_step_size_Factor;
}
/* SQUARE TUBE RAIL
TOP FACE
(P1)v2 _____ v1(P1)
/ /|
(P1)v3/|___/_| v0(P1)
/ / / /
LEFT FACE ---> /_/__/ / <--- RIGHT FACE
(P0)v2|/ | /v1(P0)
|____|/
(P0)v3 v0(P0)
BOTTOM FACE
P0: current point, P1: next point
.v2(-n+b) .v1(+n+b)
.v3(-n-b) .v0(+n-b)
RIGHT FACE : v0(P0) - v0(P1) - v1(P1) - v1(P0)
LEFT FACE : (P0)v3 - (P1)v3 - (P1)v2 -(P0)v2
TOP FACE : (P0)v2 - v1(P0) - v1(P1) - (P1)v2
BOTTOM FACE: (P0)v3 - v0(P0) - v0(P1) - (P1)v3
*/
/* V-SHAPED SQUARE TUBE RAIL
(P1)v2 _____ v1(P1) (P1)v2 _____ v1(P1)
/ /| / /|
(P1)v3/|___/_| v0(P1) (P1)v3/|___/_| v0(P1)
/ / / / / / / /
TOP_LEFT RAIL --> /_/__/ / /_/__/ / <-- TOP-RIGHT RAIL
(P0)v2|/ | /v1(P0) (P0)v2|/ | /v1(P0)
|____|/ |____|/
(P0)v3 v0(P0) (P0)v3 v0(P0)
(P1)v2 _____ v1(P1)
/ /|
(P1)v3/|___/_| v0(P1)
/ / / / <-- BOTTOM RAIL
/_/__/ /
(P0)v2|/ | /v1(P0)
|____|/
(P0)v3 v0(P0)
In drawSplines()
render tripe rail as a V-shpaed by using different start point of P0
BOTTOM RAIL: like index==1 render the first rail which lies in the bottom of V-shaped
TOP_LEFT RAIL: like index==2 render the second rail which lies in the top-left of V-shaped
TOP-RIGHT RAIL: like index==3 render the third rail which lies in the top-right of V-shaped
*/
/* p0 v0 */
glVertex3f(p.x + tube_step_size*( n.x - b.x), p.y + tube_step_size*( n.y - b.y), p.z + tube_step_size*( n.z - b.z));
/* p1 v0 */
glVertex3f(p1.x + tube_step_size*( n1.x - b1.x), p1.y + tube_step_size*( n1.y - b1.y), p1.z + tube_step_size*( n1.z - b1.z));
/* p1 v1 */
glVertex3f(p1.x + tube_step_size*(+n1.x + b.x), p1.y + tube_step_size*( n1.y + b1.y), p1.z + tube_step_size*( n1.z + b1.z));
/* p0 v1 */
glVertex3f(p.x + tube_step_size*( n.x + b.x), p.y + tube_step_size*( n.y + b.y), p.z + tube_step_size*( n.z + b.z));
/* p0 v3 */
glVertex3f(p.x + tube_step_size*(-n.x - b.x), p.y + tube_step_size*(-n.y - b.y), p.z + tube_step_size*(-n.z - b.z));
/* p1 v3 */
glVertex3f(p1.x + tube_step_size*(-n1.x - b1.x), p1.y + tube_step_size*(-n1.y - b1.y), p1.z + tube_step_size*(-n1.z - b1.z));
/* p1 v2 */
glVertex3f(p1.x + tube_step_size*(-n1.x + b1.x), p1.y + tube_step_size*(-n1.y + b1.y), p1.z + tube_step_size*(-n1.z + b1.z));
/* p0 v2 */
glVertex3f(p.x + tube_step_size*(-n.x + b.x), p.y + tube_step_size*(-n.y + b.y), p.z + tube_step_size*(-n.z + b.z));
/* left side rail form bottom quad */
/* p0 v2 */
glVertex3f(p.x + tube_step_size*(-n.x + b.x), p.y + tube_step_size*(-n.y + b.y), p.z + tube_step_size*(-n.z + b.z));
/* p0 v1 */
glVertex3f(p1.x + tube_step_size*(-n1.x - b1.x), p1.y + tube_step_size*(-n1.y - b1.y), p1.z + tube_step_size*(-n1.z - b1.z));
/* p1 v1 */
glVertex3f(p1.x + tube_step_size*(+n1.x + b.x), p1.y + tube_step_size*( n1.y + b1.y), p1.z + tube_step_size*( n1.z + b1.z));
/* p1 v2 */
glVertex3f(p1.x + tube_step_size*(-n1.x + b1.x), p1.y + tube_step_size*(-n1.y + b1.y), p1.z + tube_step_size*(-n1.z + b1.z));
/* form top quad */
/* p0 v3 */
glVertex3f(p.x + tube_step_size*(-n.x - b.x), p.y + tube_step_size*(-n.y - b.y), p.z + tube_step_size*(-n.z - b.z));
/* p0 v0 */
glVertex3f(p.x + tube_step_size*( n.x - b.x), p.y + tube_step_size*( n.y - b.y), p.z + tube_step_size*( n.z - b.z));
/* p1 v0 */
glVertex3f(p1.x + tube_step_size*( n1.x - b1.x), p1.y + tube_step_size*( n1.y - b1.y), p1.z + tube_step_size*( n1.z - b1.z));
/* p1 v3 */
glVertex3f(p1.x + tube_step_size*(-n1.x - b1.x), p1.y + tube_step_size*(-n1.y - b1.y), p1.z + tube_step_size*(-n1.z - b1.z));
}/* end of for loop*/
}/* end of for loop*/
}/* end of for loop*/
glEnd();
}/* end of for loop*/
}
void makeTracks()
{
Vector3 up = Vector3();
Vector3 currentQuad[4] = { Vector3(), Vector3(), Vector3(), Vector3() };
Vector3 right = Vector3(0, 1, 0);
float trackWidth = 0.5f;
float railSize = 0.025f;
int crossIndex = 0;
for (int i = 0; i < totalPoints - 1; i++)
{
Vector3 forward = Vector3(pointsList[i + 1].x - pointsList[i].x, pointsList[i + 1].y - pointsList[i].y, pointsList[i + 1].z - pointsList[i].z);
crossProduct(forward, right, up);
Normalize(up);
Multiply(right, railSize);
Multiply(up, railSize);
// Base vertices
Vector3 point, v0, v1, v2, v3, v4, v5, v6, v7;
if (i == 0)
{
point = Vector3(pointsList[i].x, pointsList[i].y, pointsList[i].z);
v0 = Vector3(pointsList[i].x + up.x - right.x - 75, pointsList[i].y + up.y - right.y - 30, pointsList[i].z + up.z - right.z - 6);
v1 = Vector3(pointsList[i].x - up.x - right.x - 75, pointsList[i].y - up.y - right.y - 30, pointsList[i].z - up.z - right.z - 6);
v2 = Vector3(pointsList[i].x - up.x + right.x - 75, pointsList[i].y - up.y + right.y - 30, pointsList[i].z - up.z + right.z - 6);
v3 = Vector3(pointsList[i].x + up.x + right.x - 75, pointsList[i].y + up.y + right.y - 30, pointsList[i].z + up.z + right.z - 6);
v4 = Vector3(pointsList[i + 1].x + up.x - right.x - 75, pointsList[i + 1].y + up.y - right.y - 30, pointsList[i + 1].z + up.z - right.z - 6);
v5 = Vector3(pointsList[i + 1].x - up.x - right.x - 75, pointsList[i + 1].y - up.y - right.y - 30, pointsList[i + 1].z - up.z - right.z - 6);
v6 = Vector3(pointsList[i + 1].x - up.x + right.x - 75, pointsList[i + 1].y - up.y + right.y - 30, pointsList[i + 1].z - up.z + right.z - 6);
v7 = Vector3(pointsList[i + 1].x + up.x + right.x - 75, pointsList[i + 1].y + up.y + right.y - 30, pointsList[i + 1].z + up.z + right.z - 6);
currentQuad[0] = v4;
currentQuad[1] = v5;
currentQuad[2] = v6;
currentQuad[3] = v7;
}
else
{
v0 = currentQuad[0];
v1 = currentQuad[1];
v2 = currentQuad[2];
v3 = currentQuad[3];
v4 = Vector3(pointsList[i + 1].x + up.x - right.x - 75, pointsList[i + 1].y + up.y - right.y - 30, pointsList[i + 1].z + up.z - right.z - 6);
v5 = Vector3(pointsList[i + 1].x - up.x - right.x - 75, pointsList[i + 1].y - up.y - right.y - 30, pointsList[i + 1].z - up.z - right.z - 6);
v6 = Vector3(pointsList[i + 1].x - up.x + right.x - 75, pointsList[i + 1].y - up.y + right.y - 30, pointsList[i + 1].z - up.z + right.z - 6);
v7 = Vector3(pointsList[i + 1].x + up.x + right.x - 75, pointsList[i + 1].y + up.y + right.y - 30, pointsList[i + 1].z + up.z + right.z - 6);
currentQuad[0] = v4;
currentQuad[1] = v5;
currentQuad[2] = v6;
currentQuad[3] = v7;
}
// Make the right rail vertices
Vector3 r0, r1, r2, r3, r4, r5, r6, r7;
Vector3 scale = right;
Multiply(scale, -6.0f);
r0 = Add(v0, scale);
r1 = Add(v1, scale);
r2 = Add(v2, scale);
r3 = Add(v3, scale);
r4 = Add(v4, scale);
r5 = Add(v5, scale);
r6 = Add(v6, scale);
r7 = Add(v7, scale);
// Make the left rail vertices
scale = right;
Vector3 left = right;
Multiply(scale, 7.0f);
Multiply(left, -1.0f);
left = Add(left, scale);
Vector3 l0, l1, l2, l3, l4, l5, l6, l7;
l0 = Add(v0, left);
l1 = Add(v1, left);
l2 = Add(v2, left);
l3 = Add(v3, left);
l4 = Add(v4, left);
l5 = Add(v5, left);
l6 = Add(v6, left);
l7 = Add(v7, left);
glBegin(GL_QUADS);
glColor3f(0.0f, 0.0f, 1.0f);
// Left Rail
// Left face
glVertex3f(l0.x, l0.y, l0.z);
glVertex3f(l4.x, l4.y, l4.z);
glVertex3f(l5.x, l5.y, l5.z);
glVertex3f(l1.x, l1.y, l1.z);
// Right face
glVertex3f(l3.x, l3.y, l3.z);
glVertex3f(l2.x, l2.y, l2.z);
glVertex3f(l6.x, l6.y, l6.z);
glVertex3f(l7.x, l7.y, l7.z);
// Bottom face
glVertex3f(l1.x, l1.y, l1.z);
glVertex3f(l5.x, l5.y, l5.z);
glVertex3f(l6.x, l6.y, l6.z);
glVertex3f(l2.x, l2.y, l2.z);
// Top face
glColor3f(1, 1, 1);
glVertex3f(l0.x, l0.y, l0.z);
glVertex3f(l3.x, l3.y, l3.z);
glVertex3f(l7.x, l7.y, l7.z);
glVertex3f(l4.x, l4.y, l4.z);
// Right Rail
glColor3f(0.0f, 0.0f, 1.0f);
// Left face
glVertex3f(r0.x, r0.y, r0.z);
glVertex3f(r4.x, r4.y, r4.z);
glVertex3f(r5.x, r5.y, r5.z);
glVertex3f(r1.x, r1.y, r1.z);
// Right face
glVertex3f(r3.x, r3.y, r3.z);
glVertex3f(r2.x, r2.y, r2.z);
glVertex3f(r6.x, r6.y, r6.z);
glVertex3f(r7.x, r7.y, r7.z);
// Bottom face
glVertex3f(r1.x, r1.y, r1.z);
glVertex3f(r5.x, r5.y, r5.z);
glVertex3f(r6.x, r6.y, r6.z);
glVertex3f(r2.x, r2.y, r2.z);
// Top face
glColor3f(1, 1, 1);
glVertex3f(r0.x, r0.y, r0.z);
glVertex3f(r3.x, r3.y, r3.z);
glVertex3f(r7.x, r7.y, r7.z);
glVertex3f(r4.x, r4.y, r4.z);
// Cross beam every 7 sections
if (crossIndex % 7 == 0)
{
Normalize(forward);
Multiply(forward, railSize);
v0 = l3;
v1 = l2;
v2 = r1;
v3 = r0;
v4 = Add(v0, forward);
v5 = Add(v1, forward);
v6 = Add(v2, forward);
v7 = Add(v3, forward);
glColor3f(0, 0, 1);
// Front cross face
glVertex3f(v0.x, v0.y, v0.z);
glVertex3f(v1.x, v1.y, v1.z);
glVertex3f(v2.x, v2.y, v2.z);
glVertex3f(v3.x, v3.y, v3.z);
// Back cross face
glVertex3f(v7.x, v7.y, v7.z);
glVertex3f(v6.x, v6.y, v6.z);
glVertex3f(v5.x, v5.y, v5.z);
glVertex3f(v4.x, v4.y, v4.z);
// Bottom cross face
glVertex3f(v1.x, v1.y, v1.z);
glVertex3f(v5.x, v5.y, v5.z);
glVertex3f(v6.x, v6.y, v6.z);
glVertex3f(v2.x, v2.y, v2.z);
// Top cross face
glColor3f(1, 1, 1);
glVertex3f(v0.x, v0.y, v0.z);
glVertex3f(v3.x, v3.y, v3.z);
glVertex3f(v7.x, v7.y, v7.z);
glVertex3f(v4.x, v4.y, v4.z);
}
glEnd();
Multiply(up, 2.0f);
Multiply(right, 2.0f);
crossProduct(up, forward, right);
Normalize(right);
crossIndex++;
}
}
/* implement cross rail (45 degrss '/' and 135 degree '\') between two side rails and bottom rail respeactively */
void drawCrossRail()
{
int factor=0;
float cross_step_size=0.08;
float up_step_size=0.035;
/* parallel rail number */
for(int index=2;index<4;index++){
glLineWidth(7);
glBegin(GL_LINES);
/* white */
glColor3f(1, 1, 1);
for (int spline_index = 0; spline_index < g_iNumOfSplines; spline_index++) {
for(int i=-2;i<g_Splines[spline_index].numControlPoints-1;i++) {
for(float u=0.0;u<1.0;u+=0.1) {
/*Point*/
p.x= catmullRomSpline(u, g_Splines[spline_index].points[i].x, g_Splines[spline_index].points[i+1].x, g_Splines[spline_index].points[i+2].x, g_Splines[spline_index].points[i+3].x);
p.y= catmullRomSpline(u, g_Splines[spline_index].points[i].y, g_Splines[spline_index].points[i+1].y, g_Splines[spline_index].points[i+2].y, g_Splines[spline_index].points[i+3].y);
p.z= catmullRomSpline(u, g_Splines[spline_index].points[i].z, g_Splines[spline_index].points[i+1].z, g_Splines[spline_index].points[i+2].z, g_Splines[spline_index].points[i+3].z);
/*Tangent*/
t.x= tangent(u, g_Splines[spline_index].points[i].x, g_Splines[spline_index].points[i+1].x, g_Splines[spline_index].points[i+2].x, g_Splines[spline_index].points[i+3].x);
t.y= tangent(u, g_Splines[spline_index].points[i].y, g_Splines[spline_index].points[i+1].y, g_Splines[spline_index].points[i+2].y, g_Splines[spline_index].points[i+3].y);
t.z= tangent(u, g_Splines[spline_index].points[i].z, g_Splines[spline_index].points[i+1].z, g_Splines[spline_index].points[i+2].z, g_Splines[spline_index].points[i+3].z);
t = unit(t);
/*Normal*/
n = crossProduct(t, arbitrary_point);
n = unit(n);
/*Binormal*/
b = crossProduct(t, n);
b = unit(b);
/*
(P1)v2 _____ v1(P1) (P1)v2 _____ v1(P1)
/ /| / /|
(P1)v3/|___/_| v0(P1) (P1)v3/|___/_| v0(P1)
/ / / / / / / /
/_/__/ / /_/__/ /
(P0)v2|/ | /v1(P0) (P0)v2|/ | /v1(P0)
|____|/ |____|/
(P0)v3 v0(P0) (P0)v3 v0(P0)
\ \ / /
\ \ / /
\ \ (P1)v2 _____ v1(P1) / /
CROSS RAIL SUPPORT--> \ \ / /| / / <--- CROSS RAIL SUPPORT
\ \(P1)v3/|___/_| v0(P1) / /
\ \ / / / / / /
/_/__/ / / /
(P0)v2|/ | /v1(P0)
|____|/
(P0)v3 v0(P0)
*/
/* top*/
if(index==2)
{
p.x+=up_step_size*b.x*up_step_size_Factor;
p.y+=up_step_size*b.y*up_step_size_Factor;
p.z+=up_step_size*b.z*up_step_size_Factor;
factor = 1;
}
/* top-right*/
if(index==3)
{
p.x+=cross_step_size*n.x+ up_step_size*b.x*up_step_size_Factor;
p.y+=cross_step_size*n.y+ up_step_size*b.y*up_step_size_Factor;
p.z+=cross_step_size*n.z+ up_step_size*b.z*up_step_size_Factor;
factor = -1;
}
/* side rail */
glVertex3f(p.x, p.y, p.z);
/* main bottom rail*/
glVertex3f(p.x-up_step_size*b.x*up_step_size_Factor+factor*cross_step_size*n.x/2, p.y-up_step_size*b.y*up_step_size_Factor+factor*cross_step_size*n.y/2, p.z-up_step_size*b.z*up_step_size_Factor+factor*cross_step_size*n.z/2);
}/* end of for loop*/
}/* end of for loop*/
}/* end of for loop*/
glEnd();
}/* end of for loop*/
}
/*!
\overload
Returns the normal vector of a plane defined by vectors
\a v2 - \a v1 and \a v3 - \a v1, normalized to be a unit vector.
Use crossProduct() to compute the cross-product of \a v2 - \a v1 and
\a v3 - \a v1 if you do not need the result to be normalized to a
unit vector.
\sa crossProduct(), distanceToPlane()
*/
QVector3D QVector3D::normal
(const QVector3D& v1, const QVector3D& v2, const QVector3D& v3)
{
return crossProduct((v2 - v1), (v3 - v1)).normalized();
}
int GzBeginRender(GzRender *render)
{
/*
- set up for start of each frame - clear frame buffer
- compute Xiw and projection xform Xpi from camera definition
- init Ximage - put Xsp at base of stack, push on Xpi and Xiw
- now stack contains Xsw and app can push model Xforms if it want to.
*/
/* Compoute Xiw */
GzCoord CamX, CamY, CamZ, C;
// Calculate CamZ
for (int i = 0; i < 3; i++)
{
C[i] = render->camera.position[i];
//CamY[i] = render->camera.worldup[i];
CamZ[i] = render->camera.lookat[i] - render->camera.position[i];
}
float lenZ = length(CamZ);
for (int i = 0; i < 3; i++)
CamZ[i] = CamZ[i] / lenZ;
// Calculate CamY
float dotUpZ = dotProduct(render->camera.worldup, CamZ);
for (int i = 0; i < 3; i++)
{
CamY[i] = render->camera.worldup[i] - dotUpZ * CamZ[i];
}
float lenY = length(CamY);
for (int i = 0; i < 3; i++)
CamY[i] = CamY[i] / lenY;
// Calculate CamX
float *ptr = crossProduct(CamY, CamZ);
for (int i = 0; i < 3; i++)
CamX[i] = ptr[i];
float lenX = length(CamX);
for (int i = 0; i < 3; i++)
CamX[i] = CamX[i] / lenX;
for (int i = 0; i < 3; i++)
{
render->camera.Xiw[0][i] = CamX[i];
render->camera.Xiw[1][i] = CamY[i];
render->camera.Xiw[2][i] = CamZ[i];
render->camera.Xiw[3][i] = 0;
}
render->camera.Xiw[0][3] = -(CamX[0] * C[0] + CamX[1] * C[1] + CamX[2] * C[2]);
render->camera.Xiw[1][3] = -(CamY[0] * C[0] + CamY[1] * C[1] + CamY[2] * C[2]);
render->camera.Xiw[2][3] = -(CamZ[0] * C[0] + CamZ[1] * C[1] + CamZ[2] * C[2]);
render->camera.Xiw[3][3] = 1;
/* Compute Xpi */
for (int i = 0; i < 4; i++)
memset(render->camera.Xpi[i], 0, sizeof(float)*4);
for (int i = 0; i < 4; i++)
render->camera.Xpi[i][i] = 1;
render->camera.Xpi[3][2] = tan((render->camera.FOV/2)*3.14159265/180);
/* Init Ximage */
render->matlevel = 0; // initial position of the stack
GzPushMatrix(render, render->Xsp); // push Xsp
GzPushMatrix(render, render->camera.Xpi); // push Xpi
GzPushMatrix(render, render->camera.Xiw); // push Xiw
return GZ_SUCCESS;
}
