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