// Dessine la couronne intérieure std::vector<Point_3> DegradeAnObject::drawInsideImpactOnFacet(std::vector<Point_3> points, std::vector<Halfedge_handle> hhs, Facet f, int index) { std::vector<Point_3> pts; for(int i = 0 ; i < points.size() ; i++) { int j; if(i == points.size()-1) { j = 0; } else { j = i+1; } Vector_3 h(hhs[i]->opposite()->vertex()->point(), hhs[i]->vertex()->point()); Vector_3 g(hhs[j]->opposite()->vertex()->point(), hhs[j]->vertex()->point()); Vector_3 norm = getNormalOfFacet(f); Vector_3 rh = normalizeVector(rotationVector(h, norm, M_PI/2)); Vector_3 rg = normalizeVector(rotationVector(g, norm, M_PI/2)); Vector_3 comb = 0.01*normalizeVector(rh+rg); Point_3 newPoint = hhs[i]->vertex()->point() + comb; Halfedge_handle hh = polys[index].split_vertex(hhs[j]->opposite(), hhs[i]); hh->vertex()->point() = newPoint; polys[index].split_facet(hh->opposite()->next()->next(), hh->opposite()); polys[index].split_facet(hh->next()->next(), hh); pts.push_back(newPoint); } return pts; }
void KinematicMotion::addRotation(const double *xAxisNew, const double *yAxisNew, const double *zAxisNew, bool normalized) { double e0[3]; double e1[3]; double e2[3]; copyVector(xAxisNew, e0); copyVector(yAxisNew, e1); copyVector(zAxisNew, e2); if (!normalized) { normalizeVector(e0); normalizeVector(e1); normalizeVector(e2); } double newRotation[9]; for (int i = 0; i < 3; i++) { newRotation[i * 3 + 0] = e0[i]; newRotation[i * 3 + 1] = e1[i]; newRotation[i * 3 + 2] = e2[i]; } //R'*(R*x + t) = R'*R*x + R'*t matrixProduct(newRotation, rotationMatrix); matrixVectorProduct(newRotation, translationVector); }
void Bevgraf::initViewMatrix(MATRIX4 A, Vector3D eye, Vector3D center, Vector3D up) { initIdentityMatrix(A); // a kamerabol az iranyt meghatarozo pontba mutato vektor // center az a pont,amely fele a kamerat tartjuk, eye a kamera helyét adja Vector3D centerMinusEye = initVector3(center.x - eye.x, center.y - eye.y, center.z - eye.z); // a fenti vektor -1 szeresének egyseg hosszra normáltja, ebbol lesz a kamera rendszerének z-tengelye Vector3D f = normalizeVector(initVector3(-centerMinusEye.x, -centerMinusEye.y, -centerMinusEye.z)); // az up vektor es a leendo z tengelyirany vektorialis szorzata adja a kamera x-tengelyiranyat Vector3D s = normalizeVector(crossProduct(up, f)); // a kamera y tengelyiranya a mar elkeszult z irany es x irany vektorialis szorzataként jön ki Vector3D u = crossProduct(f, s); A[0][0] = s.x; A[0][1] = s.y; A[0][2] = s.z; A[1][0] = u.x; A[1][1] = u.y; A[1][2] = u.z; A[2][0] = f.x; A[2][1] = f.y; A[2][2] = f.z; A[0][3] = -dotProduct(s, eye); A[1][3] = -dotProduct(u, eye); A[2][3] = -dotProduct(f, eye); }
float* normalOf(float p01, float p02, float p03, float p11, float p12, float p13, float p21, float p22, float p23) { float* ret = (float*)calloc(3, sizeof(float)); float p1[3]; float p2[3]; float p3[3]; p1[0] = p01; p1[1] = p02; p1[2] = p03; p2[0] = p11; p2[1] = p12; p2[2] = p13; p3[0] = p21; p3[1] = p22; p3[2] = p23; normalizeVector(p1); normalizeVector(p2); normalizeVector(p3); for(int i = 0 ; i < 3 ; ++i) { p2[i] -= p1[i]; p3[i] -= p1[i]; } for(int i = 0 ; i < 3 ; ++i) { ret[i] = p2[(i + 1) % 3] * p3[(i + 2) % 3] - p2[(i + 2) % 3] * p3[(i + 1) % 3]; } normalizeVector(ret); return ret; }
void KinematicMotion::addRotation(const double *vec0, const double *vec1, bool normalized) { // not unique, but shortest path // rotation is defined in the CURRENT coordinates system double e0[3]; copyVector(vec0, e0); double e1[3]; copyVector(vec1, e1); if (!normalized) { normalizeVector(e0); normalizeVector(e1); } // algorithm form Non-linear Modeling and Analysis of Solids and Structures (Steen Krenk) P54 double e2[3]; e2[0] = e0[0] + e1[0]; e2[1] = e0[1] + e1[1]; e2[2] = e0[2] + e1[2]; double e2_square = vectorProduct(e2, e2); double newRotation[9]; newRotation[0] = 1.0 + 2.0 * e1[0] * e0[0] - 2.0 / e2_square * e2[0] * e2[0]; newRotation[1] = 0.0 + 2.0 * e1[0] * e0[1] - 2.0 / e2_square * e2[0] * e2[1]; newRotation[2] = 0.0 + 2.0 * e1[0] * e0[2] - 2.0 / e2_square * e2[0] * e2[2]; newRotation[3] = 0.0 + 2.0 * e1[1] * e0[0] - 2.0 / e2_square * e2[1] * e2[0]; newRotation[4] = 1.0 + 2.0 * e1[1] * e0[1] - 2.0 / e2_square * e2[1] * e2[1]; newRotation[5] = 0.0 + 2.0 * e1[1] * e0[2] - 2.0 / e2_square * e2[1] * e2[2]; newRotation[6] = 0.0 + 2.0 * e1[2] * e0[0] - 2.0 / e2_square * e2[2] * e2[0]; newRotation[7] = 0.0 + 2.0 * e1[2] * e0[1] - 2.0 / e2_square * e2[2] * e2[1]; newRotation[8] = 1.0 + 2.0 * e1[2] * e0[2] - 2.0 / e2_square * e2[2] * e2[2]; //R'*(R*x + t) = R'*R*x + R'*t matrixProduct(newRotation, rotationMatrix); matrixVectorProduct(newRotation, translationVector); }
//------------------------------------------------------------------------------ void calc_specular(GzRender *render, GzCoord N_orig, GzColor col, bool mulByK) { // N is already sent here after transformation GzCoord N = {0.0f,0.0f,0.0f}; normalizeVector(N_orig, N); GzCoord AccumulatedSpecResult = {0.0f, 0.0f, 0.0f}; for(int i = 0; i < render->numlights; i++) { float (*ls)[3] = static_cast<float (*)[3]>(render->lights[i]); GzCoord ls_L_orig = {ls[0][0], ls[0][1], ls[0][2]}; GzCoord E = {0, 0, -1}; GzCoord ls_L = {0.0f,0.0f,0.0f}; normalizeVector(ls_L_orig, ls_L); if (!shouldCalcLight(N, ls_L, E, N)) continue; float N_dot_L = vectorDotProduct(N, ls_L); GzCoord left = {0.0f,0.0f,0.0f}; scalarMultiply(N, N_dot_L * 2.0f, left); GzCoord R_orig = {0.0f,0.0f,0.0f}; subtractVector(left, ls_L, R_orig); GzCoord R = {0.0f,0.0f,0.0f}; normalizeVector(R_orig, R); GzCoord ls_intensity = {ls[1][0], ls[1][1], ls[1][2]}; GzCoord localResult = {0.0f,0.0f,0.0f}; float RdotE = vectorDotProduct(R,E); if (RdotE < 0) RdotE = 0; if (RdotE > 1) RdotE = 1; scalarMultiply(ls_intensity, powf(RdotE, render->spec), localResult); addVectors(localResult, AccumulatedSpecResult, AccumulatedSpecResult); } if(mulByK) vectorMulElementByElement(render->Ks, AccumulatedSpecResult, col); else { col[RED] = AccumulatedSpecResult[RED]; col[GREEN] = AccumulatedSpecResult[GREEN]; col[BLUE] = AccumulatedSpecResult[BLUE]; } }
/** * @brief planeOrthonormalBase compute a orthonormal base of the plane expressed in the reference frame so that * the Z axis of the new base corresponds to the normal vector to the plane * @param planeCoeff * @param v orientations of the X,Y and Z vectors of the new base expressed in the reference one */ void planeOrthonormalBase(const pcl::ModelCoefficientsPtr& planeCoeff, std::vector<double> (&v)[3]) { double A = planeCoeff->values[0]; double B = planeCoeff->values[1]; double C = planeCoeff->values[2]; double D = planeCoeff->values[3]; //first vector of the base: plane normal Z = (A, B, C) v[2].push_back(A); v[2].push_back(B); v[2].push_back(C); normalizeVector(v[2]); A = v[2][0]; B = v[2][1]; C = v[2][2]; //second vector: //if |C| > |A| and |C| > |B|: // vector: ( X=1, Y=0, Z=-(D+A)/C ) //if |A| > |B| and |A| > |C|: // vector: ( X=-(B+D)/A, Y=1, Z=0 ) //if |B| > |A| and |B| > |C|: // vector: ( X=1, Y=-(A+D)/B, Z=0 ) if ( fabs(C) >= fabs(A) && fabs(C) >= fabs(B) ) { v[0].push_back(1); v[0].push_back(0); v[0].push_back(-A/C); } else if ( fabs(A) >= fabs(B) && fabs(A) >= fabs(C) ) { v[0].push_back(-B/A); v[0].push_back(1); v[0].push_back(0); } else if ( fabs(B) >= fabs(A) && fabs(B) >= fabs(C) ) { v[0].push_back(1); v[0].push_back(-A/B); v[0].push_back(0); } normalizeVector(v[0]); //third vector: cross vector of the first two v[1].push_back(v[2][2]*v[0][1] - v[2][1]*v[0][2]); // n_z·v_y - n_y·v_z v[1].push_back(v[2][0]*v[0][2] - v[2][2]*v[0][0]); // n_x·v_z - n_z·v_x v[1].push_back(v[2][1]*v[0][0] - v[2][0]*v[0][1]); // n_y·v_x - n_x·v_y normalizeVector(v[1]); //Compute a point of the plane }
void KinematicMotion::addRotation(const double *axis, bool normalized, double angle) { // rotation is defined in the CURRENT coordinates system double u[3]; copyVector(axis, u); if (!normalized) { normalizeVector(u); } // algorithm from http://en.wikipedia.org/wiki/Rotation_matrix double c = cos(angle); double s = sin(angle); double newRotation[9]; newRotation[0] = c + u[0] * u[0] * (1.0 - c); newRotation[1] = u[0] * u[1] * (1.0 - c) - u[2] * s; newRotation[2] = u[0] * u[2] * (1.0 - c) + u[1] * s; newRotation[3] = u[1] * u[0] * (1.0 - c) + u[2] * s; newRotation[4] = c + u[1] * u[1] * (1.0 - c); newRotation[5] = u[1] * u[2] * (1.0 - c) - u[0] * s; newRotation[6] = u[2] * u[0] * (1.0 - c) - u[1] * s; newRotation[7] = u[2] * u[1] * (1.0 - c) + u[0] * s; newRotation[8] = c + u[2] * u[2] * (1.0 - c); //R'*(R*x + t) = R'*R*x + R'*t matrixProduct(newRotation, rotationMatrix); matrixVectorProduct(newRotation, translationVector); }
void ComplementaryFilter::getMeasurement( double ax, double ay, double az, double& q0_meas, double& q1_meas, double& q2_meas, double& q3_meas) { // q_acc is the quaternion obtained from the acceleration vector representing // the orientation of the Global frame wrt the Local frame with arbitrary yaw // (intermediary frame). q3_acc is defined as 0. // Normalize acceleration vector. normalizeVector(ax, ay, az); if (az >=0) { q0_meas = sqrt((az + 1) * 0.5); q1_meas = -ay/(2.0 * q0_meas); q2_meas = ax/(2.0 * q0_meas); q3_meas = 0; } else { double X = sqrt((1 - az) * 0.5); q0_meas = -ay/(2.0 * X); q1_meas = X; q2_meas = 0; q3_meas = ax/(2.0 * X); } }
int GzPutCamera(GzRender *render, GzCamera *camera) { /* - overwrite renderer camera structure with new camera definition */ if (render == NULL) { return GZ_FAILURE; } if (camera == NULL) { return GZ_FAILURE; } //render->camera = camera; render->camera.FOV = camera->FOV; render->camera.lookat[X] = camera->lookat[X]; render->camera.lookat[Y] = camera->lookat[Y]; render->camera.lookat[Z] = camera->lookat[Z]; render->camera.position[X] = camera->position[X]; render->camera.position[Y] = camera->position[Y]; render->camera.position[Z] = camera->position[Z]; render->camera.worldup[X] = camera->worldup[X]; render->camera.worldup[Y] = camera->worldup[Y]; render->camera.worldup[Z] = camera->worldup[Z]; normalizeVector(render->camera.worldup); // I should have to redo this right since camera.fov's changed render->Xsp[2][2] = 2147483647 * tan((render->camera.FOV / 2.0) * (PI / 180.0)); return GZ_SUCCESS; }
point Actor::computeVertexNormal (point a, point b, point c, point d) { point value; value.x = (a.x + b.x + c.x + d.x) / 4; value.y = (a.y + b.y + c.y + d.y) / 4; value.z = (a.z + b.z + c.z + d.z) / 4; return normalizeVector(value); }
float StlSphere::getArea(Facet *facet) { float cross[3][3]; float sum[3]; //. float n[3]; for (int i = 0; i < 3; i++) { cross[i][0] = ((facet->vector[i].y * facet->vector[(i + 1) % 3].z) - (facet->vector[i].z * facet->vector[(i + 1) % 3].y)); cross[i][1] = ((facet->vector[i].z * facet->vector[(i + 1) % 3].x) - (facet->vector[i].x * facet->vector[(i + 1) % 3].z)); cross[i][2] = ((facet->vector[i].x * facet->vector[(i + 1) % 3].y) - (facet->vector[i].y * facet->vector[(i + 1) % 3].x)); } sum[0] = cross[0][0] + cross[1][0] + cross[2][0]; sum[1] = cross[0][1] + cross[1][1] + cross[2][1]; sum[2] = cross[0][2] + cross[1][2] + cross[2][2]; // This should already be done. But just in case, let's do it again //calculateNormal(n, facet); //normalizeVector(n); //float area = 0.5 * (n[0] * sum[0] + n[1] * sum[1] + n[2] * sum[2]); calculateNormal( facet); normalizeVector(&facet->normal); float area = 0.5 * (facet->normal.x * sum[0] + facet->normal.y * sum[1] + facet->normal.z * sum[2]); return area; }
float distancePointLineOnPlane (vector_t const point, const line_t line, const plane_t plane) { vector_t normal_in_plane = normalizeVector(crossProduct( line.direction, plane.normal)); return innerProduct(subtractVectors(point,line.point),normal_in_plane); }
line_t initLine (vector_t direction, const vector_t point) { line_t line; line.point = point; line.direction = normalizeVector(direction); return line; }
//****************************************************************** //FUNCTION: void COutdoorLightScattering::computeInscatteringIntegral(vec3f vRayStart, vec3f vRayEnd, vec3f vEarthCentre, vec3f vDirOnLight, vec2f& voNetParticleFromCam, vec3f& voRayleighInscattering, vec3f& voMieInscattering, const float vNumSteps /*= 7*/) { voNetParticleFromCam[0] = voNetParticleFromCam[1] = 0.0; voRayleighInscattering[0] = voRayleighInscattering[1] = voRayleighInscattering[2] = 0.0; voMieInscattering[0] = voMieInscattering[1] = voMieInscattering[2] = 0.0; normalizeVector(vDirOnLight); computeScatteringCoefficients(); vec3f Steps = (vRayEnd - vRayStart) / vNumSteps; float StepLen = sqrt(Steps[0]*Steps[0] + Steps[1]*Steps[1] + Steps[2]*Steps[2]); for (float StepNum = 0.5; StepNum < vNumSteps; StepNum += 1.0) { vec3f CurrPos = vRayStart + Steps * StepNum; vec2f ParticleDensity, NetParticleDensityToAtmTop; __getAtmosphereProperties(CurrPos, vEarthCentre, vDirOnLight, ParticleDensity, NetParticleDensityToAtmTop); NetParticleDensityToAtmTop = __IntegrateParticleDensityAlongRay(CurrPos, vDirOnLight, vEarthCentre); voNetParticleFromCam += ParticleDensity * StepLen; vec3f RlghInsctr, MieInsctr; RlghInsctr[0] = RlghInsctr[1] = RlghInsctr[2] = 0.0; MieInsctr[0] = MieInsctr[1] = MieInsctr[2] = 0.0; __computePointDiffInscattering(ParticleDensity, voNetParticleFromCam, NetParticleDensityToAtmTop, RlghInsctr, MieInsctr); voRayleighInscattering += RlghInsctr * StepLen; voMieInscattering += MieInsctr * StepLen; } float CosBetha = computeCosBetha(vRayStart - vRayEnd, vDirOnLight); __applyPhaseFunction(voRayleighInscattering, voMieInscattering, CosBetha); vec3f LightInScattering = voRayleighInscattering + voMieInscattering; }
plane_t initPlane (vector_t normal, const vector_t point) { plane_t plane; plane.point = point; plane.normal = normalizeVector(normal); return plane; }
Point* findFourthVertex(Point * a, Point *b ,Point *c ){ //http://math.stackexchange.com/questions/463867/find-the-corner-of-a-tetrahedron //https://answers.yahoo.com/question/index?qid=20091025020201AAYTQrJ //Want to find Perpendicular bisector plane for AB and AC //Find legnth of side. Vector * ab = subPointsNew(a,b); float side = magnitude(ab); free(ab); //Finding PBP for AB Point * centroid = findCentroid(a,b,c); Plane * pbp_ab = findPBP(a,b); Plane * pbp_ac = findPBP(a,c); //Finding plane intersection line Vector * dirVect = crossProduct((Point *)pbp_ab, (Point*) pbp_ac); //Calculate direction vector normalizeVector(dirVect, magnitude(dirVect)); //Want to calculate amount to extend from centroid to get forth point scalePoint(dirVect, side * SIDE_TO_AH_RATIO); Point * ans = addPointsNew(dirVect, centroid); free(centroid); free(dirVect); free(pbp_ab); free(pbp_ac); return ans; }
plane_t planeFromThreePoints(const vector_t a, const vector_t b, const vector_t c) { vector_t ab = subtractVectors(b,a); vector_t ac = subtractVectors(c,a); vector_t n = normalizeVector(crossProduct(ab,ac)); return initPlane(n,a); }
void Robot::drawLabel() { glPushMatrix(); dVector3 pos; float fr_r,fr_b,fr_n;w->g->getFrustum(fr_r,fr_b,fr_n); const float txtWidth = 12.0f*fr_r/(float)w->g->getWidth(); const float txtHeight = 24.0f*fr_b/(float)w->g->getHeight(); pos[0] = dBodyGetPosition(chassis->body)[0]; pos[1] = dBodyGetPosition(chassis->body)[1]; pos[2] = dBodyGetPosition(chassis->body)[2]; float xyz[3],hpr[3]; w->g->getViewpoint(xyz,hpr); float ax = -pos[0]+xyz[0]; float ay = -pos[1]+xyz[1]; float az = -pos[2]+xyz[2]; float fx,fy,fz; float rx,ry,rz; w->g->getCameraForward(fx,fy,fz); w->g->getCameraRight(rx,ry,rz); normalizeVector(fx,fy,fz); normalizeVector(rx,ry,rz); float zz = fx*ax + fy*ay + fz*az; float zfact = zz/fr_n; pos[2] += cfg->robotSettings.RobotHeight*0.5f + cfg->robotSettings.BottomHeight + cfg->robotSettings.WheelRadius + txtHeight*zfact; dMatrix3 rot; dRFromAxisAndAngle(rot,0,0,0,0); float tx = fy*rz-ry*fz; float ty = rx*fz-fx*rz; float tz = fx*ry-fy*rx; w->g->setTransform(pos,rot); w->g->useTexture((m_rob_id-1) + 11 + 10*((on)?0:1)); glShadeModel (GL_FLAT); glDisable(GL_LIGHTING); glEnable(GL_BLEND); glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA); glBegin(GL_QUADS); glTexCoord2f(1,1);glVertex3f( txtWidth*rx*zfact +txtHeight*tx*zfact, txtWidth*ry*zfact +txtHeight*ty*zfact, txtWidth*rz*zfact +txtHeight*tz*zfact); glTexCoord2f(0,1);glVertex3f(-txtWidth*rx*zfact +txtHeight*tx*zfact,-txtWidth*ry*zfact +txtHeight*ty*zfact,-txtWidth*rz*zfact +txtHeight*tz*zfact); glTexCoord2f(0,0);glVertex3f(-txtWidth*rx*zfact -txtHeight*tx*zfact,-txtWidth*ry*zfact -txtHeight*ty*zfact,-txtWidth*rz*zfact -txtHeight*tz*zfact); glTexCoord2f(1,0);glVertex3f( txtWidth*rx*zfact -txtHeight*tx*zfact, txtWidth*ry*zfact -txtHeight*ty*zfact, txtWidth*rz*zfact -txtHeight*tz*zfact); glEnd(); glDisable(GL_BLEND); w->g->noTexture(); glPopMatrix(); }
TVector doNormalize(TVector vec) { TVector copy = createCopyVector(vec); if (copy == NULL) return NULL; normalizeVector(copy); return copy; }
/** * Transforma a matriz de entrada em uma matriz estocástica: * a - Se uma linha da matriz é nula todos os elementos são substituidos por 1. * b - A soma de todos os elementos de uma linha deve dar 1. * @param matrix Ponteiro para uma matriz. * @param lines Quantidade de linhas na dada matriz. * @param columms Quantidade de colunas na dada matriz. */ void stochasticMatrix(double **matrix, unsigned lines, unsigned columms) { unsigned i; for (i = 0; i < lines; i++) { if (isNullVector(matrix[i], columms) == true) setVectorToOne(matrix[i], columms); normalizeVector(matrix[i], columms); } }
void Particle::init(glm::vec3 basePosVector, glm::vec3 aDirectionVector, glm::vec3 normVector) { this->basePositionVector = basePosVector; this->vertexArrayIDParticle = 0; this->positionVector.resize(3); this->directionVector = normalizeVector(aDirectionVector); setPeak(basePosVector); setBasePositions(basePosVector, normVector); this->velocity = VELOCITY; }
void myGluLookAt(GLdouble eyeX, GLdouble eyeY, GLdouble eyeZ, GLdouble centerX, GLdouble centerY, GLdouble centerZ, GLdouble upX, GLdouble upY, GLdouble upZ) { if (!callingMyGl) { gluLookAt(eyeX, eyeY, eyeZ, centerX, centerY, centerZ, upX, upY, upZ); } else { GLdouble forwardVector[] = {centerX - eyeX, centerY - eyeY, centerZ - eyeZ}; GLdouble upVector[] = {upX, upY, upZ}; GLdouble sideVector[3]; normalizeVector(forwardVector); normalizeVector(upVector); computeCrossProductOfVectors(forwardVector, upVector, sideVector); normalizeVector(sideVector); computeCrossProductOfVectors(sideVector, forwardVector, upVector); GLfloat* mMatrix = new GLfloat[16]; mMatrix[0] = sideVector[0]; mMatrix[1] = upVector[0]; mMatrix[2] = -forwardVector[0]; mMatrix[3] = 0.0; mMatrix[4] = sideVector[1]; mMatrix[5] = upVector[1]; mMatrix[6] = -forwardVector[1]; mMatrix[7] = 0.0; mMatrix[8] = sideVector[2]; mMatrix[9] = upVector[2]; mMatrix[10] = -forwardVector[2]; mMatrix[11] = 0.0; mMatrix[12] = 0.0; mMatrix[13] = 0.0; mMatrix[14] = 0.0; mMatrix[15] = 1.0; loadMatrix(mMatrix); myGlTranslatef(-eyeX, -eyeY, -eyeZ); } }
void ComplementaryFilter::getMeasurement( double ax, double ay, double az, double mx, double my, double mz, double& q0_meas, double& q1_meas, double& q2_meas, double& q3_meas) { // q_acc is the quaternion obtained from the acceleration vector representing // the orientation of the Global frame wrt the Local frame with arbitrary yaw // (intermediary frame). q3_acc is defined as 0. double q0_acc, q1_acc, q2_acc, q3_acc; // Normalize acceleration vector. normalizeVector(ax, ay, az); if (az >=0) { q0_acc = sqrt((az + 1) * 0.5); q1_acc = -ay/(2.0 * q0_acc); q2_acc = ax/(2.0 * q0_acc); q3_acc = 0; } else { double X = sqrt((1 - az) * 0.5); q0_acc = -ay/(2.0 * X); q1_acc = X; q2_acc = 0; q3_acc = ax/(2.0 * X); } // [lx, ly, lz] is the magnetic field reading, rotated into the intermediary // frame by the inverse of q_acc. // l = R(q_acc)^-1 m double lx = (q0_acc*q0_acc + q1_acc*q1_acc - q2_acc*q2_acc)*mx + 2.0 * (q1_acc*q2_acc)*my - 2.0 * (q0_acc*q2_acc)*mz; double ly = 2.0 * (q1_acc*q2_acc)*mx + (q0_acc*q0_acc - q1_acc*q1_acc + q2_acc*q2_acc)*my + 2.0 * (q0_acc*q1_acc)*mz; // q_mag is the quaternion that rotates the Global frame (North West Up) into // the intermediary frame. q1_mag and q2_mag are defined as 0. double gamma = lx*lx + ly*ly; double beta = sqrt(gamma + lx*sqrt(gamma)); double q0_mag = beta / (sqrt(2.0 * gamma)); double q3_mag = ly / (sqrt(2.0) * beta); // The quaternion multiplication between q_acc and q_mag represents the // quaternion, orientation of the Global frame wrt the local frame. // q = q_acc times q_mag quaternionMultiplication(q0_acc, q1_acc, q2_acc, q3_acc, q0_mag, 0, 0, q3_mag, q0_meas, q1_meas, q2_meas, q3_meas ); //q0_meas = q0_acc*q0_mag; //q1_meas = q1_acc*q0_mag + q2_acc*q3_mag; //q2_meas = q2_acc*q0_mag - q1_acc*q3_mag; //q3_meas = q0_acc*q3_mag; }
static void quaternionFromAxis(float angle, float axis[3], float quaternion[4]){ normalizeVector(axis,3); float qangle = angle * 0.5; float sinQAngle = sin(qangle); quaternion[0] = cos(qangle); quaternion[1] = axis[0]*sinQAngle; quaternion[2] = axis[1]*sinQAngle; quaternion[3] = axis[2]*sinQAngle; }
void lookAtModel() { if(model) { cam.look[0] = model->vertices[3] - cam.eye[0]; cam.look[1] = model->vertices[4] - cam.eye[1]; cam.look[2] = model->vertices[5] - cam.eye[2]; normalizeVector(&cam.look); cam.rotateViewingDirection(0,0,0); // erzeugt neuen Linksvektor } }
void moveObjects() { Vector direction(3,0); float distance = 0.0f; int newElapsedTime = glutGet(GLUT_ELAPSED_TIME); deltaT = (newElapsedTime - elapsedTime) * 0.001; deltaT = 1.0; elapsedTime = newElapsedTime; for(int j = 0; j < bcount; j++) { if(!bowls[j].fixed) { bowls[j].pace[0] += gAcc[0]; bowls[j].pace[1] += gAcc[1]; bowls[j].pace[2] += gAcc[2]; } for(int i = 0; i < bcount; i++) { break; if(i == j) { continue; } // distanz zwischen schwerpunkt und objekt direction[0] = bowls[i].pos[0] - bowls[i].pos[0]; direction[1] = bowls[i].pos[1] - bowls[i].pos[1]; direction[2] = bowls[i].pos[2] - bowls[i].pos[2]; distance = sqrtf( direction[0] * direction[0] + direction[1] * direction[1] + direction[2]*direction[2]); normalizeVector(&direction); if(distance < 1.0f) { distance = 1; } // freifelddämpfung bowls[j].pace[0] += (bowls[i].gravity * direction[0]) / (distance*distance); bowls[j].pace[1] += (bowls[i].gravity * direction[1]) / (distance*distance); bowls[j].pace[2] += (bowls[i].gravity * direction[2]) / (distance*distance); } bowls[j].updatePosition(deltaT); } }
//------------------------------------------------------------------------------ void calc_diffuse(GzRender *render, GzCoord N_orig, GzColor col, bool mulByK) { // N is already Ncm transformed. GzCoord N = {0.0f,0.0f,0.0f}; normalizeVector(N_orig, N); GzCoord AccumulatedDiffuseResult = {0.0f, 0.0f, 0.0f}; for(int i = 0; i < render->numlights; i++) { float (*ld)[3] = static_cast<float (*)[3]>(render->lights[i]); GzCoord ld_L_orig = {ld[0][0], ld[0][1], ld[0][2]}; GzCoord ld_L = {0.0f,0.0f,0.0f}; normalizeVector(ld_L_orig, ld_L); GzCoord E = {0,0,-1}; if (!shouldCalcLight(N, ld_L, E, N)) continue; float N_dot_L = vectorDotProduct(N, ld_L); GzCoord ld_intensity = {ld[1][0], ld[1][1], ld[1][2]}; GzCoord localResult = {0.0f,0.0f,0.0f}; scalarMultiply(ld_intensity, N_dot_L, localResult); addVectors(localResult, AccumulatedDiffuseResult, AccumulatedDiffuseResult); } if(mulByK) vectorMulElementByElement(render->Kd, AccumulatedDiffuseResult, col); else { col[RED] = AccumulatedDiffuseResult[RED]; col[GREEN] = AccumulatedDiffuseResult[GREEN]; col[BLUE] = AccumulatedDiffuseResult[BLUE]; } }
void myLookAt(GLdouble eyeX, GLdouble eyeY, GLdouble eyeZ, GLdouble centerX, GLdouble centerY, GLdouble centerZ, GLdouble upX, GLdouble upY, GLdouble upZ) { GLdouble cz[3]; GLdouble cy[3]; GLdouble cx[3]; GLdouble up[3] = {upX, upY, upZ}; GLdouble minEye[3] = {-eyeX, -eyeY, -eyeZ}; // cz is the vector Pcamera to Plookat cz[0] = eyeX - centerX; cz[1] = eyeY - centerY; cz[2] = eyeZ - centerZ; // Normalize cz normalizeVector(cz); // cx = up x cz crossProduct(up, cz, cx); // Normalize cy normalizeVector(cx); // cy = cz x cx crossProduct(cz, cx, cy); GLfloat RT[16] = { cx[0], cy[0], cz[0], 0, cx[1], cy[1], cz[1], 0, cx[2], cy[2], cz[2], 0, 0 , 0 , 0 , 1 }; glTranslatef(inProduct(minEye, cx), inProduct(minEye, cy), inProduct(minEye, cz)); glMultMatrixf(RT); }
Point * findUnitDirVector(Point * a, Point *b){ Point * n = malloc(sizeof(Point)); n->x = (a->x - b->x); n->y = (a->y - b->y); n->z = (a->z - b->z); int i = gcd(abs(n->x), gcd( abs(n->y),abs(n->z))); if( i != 1){ normalizeVector(n, i); } printPoint("find dir n" , n); return n; }