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; }