void GSOCells::runEvent(Particle& part, const double) const { Sim->dynamics->updateParticle(part); Vector CellOrigin; size_t ID(part.getID()); for (size_t iDim(0); iDim < NDIM; ++iDim) { CellOrigin[iDim] = (ID % cuberootN) * cellDimension[iDim] - 0.5*Sim->primaryCellSize[iDim]; ID /= cuberootN; } //Determine the cell transition direction, its saved int cellDirectionInt(Sim->dynamics-> getSquareCellCollision3 (part, CellOrigin, cellDimension)); size_t cellDirection = abs(cellDirectionInt) - 1; GlobalEvent iEvent(getEvent(part)); #ifdef DYNAMO_DEBUG if (std::isnan(iEvent.getdt())) M_throw() << "A NAN Interaction collision time has been found" << iEvent.stringData(Sim); if (iEvent.getdt() == HUGE_VAL) M_throw() << "An infinite Interaction (not marked as NONE) collision time has been found\n" << iEvent.stringData(Sim); #endif Sim->systemTime += iEvent.getdt(); Sim->ptrScheduler->stream(iEvent.getdt()); Sim->stream(iEvent.getdt()); Vector vNorm(0,0,0); Vector pos(part.getPosition()), vel(part.getVelocity()); Sim->BCs->applyBC(pos, vel); vNorm[cellDirection] = (cellDirectionInt > 0) ? -1 : +1; //Run the collision and catch the data NEventData EDat(Sim->dynamics->runPlaneEvent(part, vNorm, 1.0, 0.0)); Sim->_sigParticleUpdate(EDat); //Now we're past the event update the scheduler and plugins Sim->ptrScheduler->fullUpdate(part); for (shared_ptr<OutputPlugin> & Ptr : Sim->outputPlugins) Ptr->eventUpdate(iEvent, EDat); }
bool hitTriang(Photon *photon, Triang *triang, const float distance, const Material *materials) { Shader shader = DIFFUSE; // Better idea? if (fDot(triang->direct, photon->direct) > -EPSILON) triang->direct = vNeg(triang->direct); // Update ray's position to the point where the ray hits the sphere photon->origin = fFMA(photon->direct, distance - UM(10), photon->origin); photon->lambda = materials[triang->mID].lambda; if (shader == CRAZY) { photon->direct = vNorm(vSub(newVec(0.99, 0, 0), photon->origin)); return REFLECTED; } else if (shader == SPECULAR) { // Angle between new and old vector is twice the angle between old vector and normal Vec reflect = vDot(photon->direct, triang->direct); photon->direct = vSub(photon->direct, vMul(triang->direct, vAdd(reflect, reflect))); return REFLECTED; } else if (shader == DIFFUSE) { photon->direct = randDir(); if (fDot(photon->direct, triang->direct) < EPSILON) photon->direct = vNeg(photon->direct); return REFLECTED; } else return ABSORBED; }
void IMU::orthonormalize(float inputDCM[3][3]) { // Takes 700 us. // Orthogonalize the i and j unit vectors (DCMDraft2 Eqn. 19). errDCM = vDotP(inputDCM[0], inputDCM[1]); vScale(inputDCM[1], -errDCM/2, dDCM[0]); // i vector correction vScale(inputDCM[0], -errDCM/2, dDCM[1]); // j vector correction vAdd(inputDCM[0], dDCM[0], inputDCM[0]); vAdd(inputDCM[1], dDCM[1], inputDCM[1]); // k = i x j vCrossP(inputDCM[0], inputDCM[1], inputDCM[2]); // Normalize all three vectors. vNorm(inputDCM[0]); vNorm(inputDCM[1]); vNorm(inputDCM[2]); }
HiVector DriftCorrect::Normalize(const HiVector &v) { HiVector vNorm(v.dim()); double v0 = v[0]; for ( int i = 0 ; i < v.dim() ; i++ ) { vNorm[i] = v[i] - v0; } _history.add("Normalize[" + ToString(v0) + "]"); return (vNorm); }
CSelectSrv::CSelectSrv():CBMWnd() { m_ppxSrvBtn = NULL; D3DVECTOR vNorm(0, 0, -1); m_avBillBoard[0] = D3DVERTEX(D3DVECTOR(-0.5f, 0.5f, 0), vNorm, 0, 0); m_avBillBoard[1] = D3DVERTEX(D3DVECTOR(-0.5f,-0.5f, 0), vNorm, 0, 1); m_avBillBoard[2] = D3DVERTEX(D3DVECTOR( 0.5f, 0.5f, 0), vNorm, 1, 0); m_avBillBoard[3] = D3DVERTEX(D3DVECTOR( 0.5f,-0.5f, 0), vNorm, 1, 1); }
/*********************************************************************** * Character * checkMeleeShot ***********************************************************************/ void fired::Character::checkMeleeShot(fired::MeleeShot *shot) { if (dead) return; sf::FloatRect ray(shot->pos, shot->direction); sf::Vector2f c, n, c2, n2; float dist; if (lineBoxCollision(phys.rect, ray, &c, &n, &dist)) { if (!isEnemy(shot->fraction)) return; phys.velocity += vNorm(shot->direction) * shot->knockback; world->addBloodSplash(c, n * 200.0f, 20); damage(shot->damage, c, shot->knockback, shot->owner); if (world->isCharExists(shot->owner)) if (dead) shot->owner->gainXP(stats.maxHP); } }
VOID CImageHandler::InitAllImage() { for ( INT nCnt = 0; nCnt < _MAX_IMAGE; nCnt++ ) m_xImageList[nCnt].Init(); ZeroMemory(m_nLoadedMagic, sizeof(INT)*_MAX_MAGIC); D3DVECTOR vNorm(0, 0, -1); m_avBillBoard[0] = D3DVERTEX(D3DVECTOR(-0.5f, 0.5f, 0), vNorm, 0, 0); m_avBillBoard[1] = D3DVERTEX(D3DVECTOR(-0.5f,-0.5f, 0), vNorm, 0, 1); m_avBillBoard[2] = D3DVERTEX(D3DVECTOR( 0.5f, 0.5f, 0), vNorm, 1, 0); m_avBillBoard[3] = D3DVERTEX(D3DVECTOR( 0.5f,-0.5f, 0), vNorm, 1, 1); for ( nCnt = 0; nCnt < _MAX_TEXTR_FILE; nCnt++ ) { m_xTextrFileList[nCnt].ClearAllNodes(); } }
//Do collision test for all entities in the geometry manager exept those to avoid void BaseTrace( rpm_file_t *pObject, CTraceline *pTrace ) { //maybe do a check if we can reach bounding box? //normalizations are expensive! pTrace->vDir.Normalize(); pTrace->bHit = false; pTrace->vEnd = pTrace->vStart + pTrace->vDir * pTrace->fMaxLength; //pre-set end point, it is overwritten in case we hit something float fShortestDist = pTrace->fMaxLength; for( UINT i = 0; i < pObject->header.num_of_triangles; i++ ) { //Ineffective! Vector3f vTriVerts[3]; vTriVerts[0].Init( pObject->getTriangles()[i].vertex3[0].pos3 ); vTriVerts[1].Init( pObject->getTriangles()[i].vertex3[1].pos3 ); vTriVerts[2].Init( pObject->getTriangles()[i].vertex3[2].pos3 ); //vTriVerts[0] -= pTrace->vStart; //vTriVerts[1] -= pTrace->vStart; //vTriVerts[2] -= pTrace->vStart; Vector3f vNorm( pObject->getTriangles()[i].norm3 ); if( LineTriangleIntersect( pTrace->vStart, pTrace->vDir, fShortestDist, //use shortest dist because we only want to find points that are nearer than //the one we already found! vTriVerts, vNorm, &pTrace->vEnd, &pTrace->fResultDist ) ) { if( pTrace->fResultDist < fShortestDist ) { pTrace->bHit = true; pTrace->vNorm = vNorm; fShortestDist = pTrace->fResultDist; } } } }
/*********************************************************************** * vSetLen ***********************************************************************/ sf::Vector2f vSetLen(sf::Vector2f v, float l) { return vNorm(v) * l; }
void renderFrame(XStuff* xs, GameState* gs, InputState* is) { //mModel = IDENT_MATRIX; gs->sunNormal.x = cos(gs->frameTime * gs->sunSpeed); gs->sunNormal.y = sin(gs->frameTime * gs->sunSpeed); gs->sunNormal.z = 0.0; //mScale3f(10, 10, 10, &mModel); //mRot3f(0, 1, 0, gs->direction, &mModel); msPush(&gs->proj); msPerspective(60, gs->screen.aspect, nearPlane, farPlane, &gs->proj); msPush(&gs->view); // order matters! don't mess with this. msTrans3f(0, -1, gs->zoom, &gs->view); msRot3f(1, 0, 0, F_PI / 6, &gs->view); msRot3f(0,1,0, gs->direction, &gs->view); msTrans3f(-gs->lookCenter.x, 0, -gs->lookCenter.y, &gs->view); // y-up to z-up rotation msRot3f(1, 0, 0, F_PI_2, &gs->view); msScale3f(1, 1, -1, &gs->view); // calculate cursor position Vector cursorp; Vector eyeCoord; Vector worldCoord; Matrix p, invp, invv; // device space (-1:1) Vector devCoord; devCoord.x = 0.50; devCoord.y = 0.50; devCoord.z = -1.0; // eye space mInverse(msGetTop(&gs->proj), &invp); vMatrixMul(&devCoord, &invp, &eyeCoord); vNorm(&eyeCoord, &eyeCoord); // world space mInverse(msGetTop(&gs->view), &invv); vMatrixMul(&eyeCoord, &invv, &worldCoord); vNorm(&worldCoord, &worldCoord); // mFastMul(msGetTop(&gs->view), msGetTop(&gs->proj), &mp); // mInverse(&mp, &invmp); // vMatrixMul(&devCoord, &invmp, &cursorp); //printf("(%f, %f, %f)\n", worldCoord.x, worldCoord.y, worldCoord.z); Vector2 c2; c2.x = 300; //cursorp.x; c2.y = 300; //cursorp.z; // draw terrain // drawTerrainBlock(&gs->map, msGetTop(&gs->model), msGetTop(&gs->view), msGetTop(&gs->proj), &gs->cursorPos); drawTerrain(&gs->map, msGetTop(&gs->view), msGetTop(&gs->proj), &gs->cursorPos, &gs->screen.wh); renderMarker(gs, 0,0); msPop(&gs->view); msPop(&gs->proj); glUseProgram(textProg->id); // text stuff textProj = IDENT_MATRIX; textModel = IDENT_MATRIX; mOrtho(0, gs->screen.aspect, 0, 1, -1, 100, &textProj); //mScale3f(.5,.5,.5, &textProj); mScale3f(.06, .06, .06, &textModel); GLuint tp_ul = glGetUniformLocation(textProg->id, "mProj"); GLuint tm_ul = glGetUniformLocation(textProg->id, "mModel"); GLuint ts_ul = glGetUniformLocation(textProg->id, "fontTex"); glUniformMatrix4fv(tp_ul, 1, GL_FALSE, textProj.m); glUniformMatrix4fv(tm_ul, 1, GL_FALSE, textModel.m); glexit("text matrix uniforms"); glDisable(GL_CULL_FACE); glActiveTexture(GL_TEXTURE0); glexit("active texture"); glUniform1i(ts_ul, 0); glexit("text sampler uniform"); glBindTexture(GL_TEXTURE_2D, arial->textureID); glexit("bind texture"); glBindVertexArray(strRI->vao); glexit("text vao bind"); glBindBuffer(GL_ARRAY_BUFFER, strRI->vbo); glexit("text vbo bind"); glDrawArrays(GL_TRIANGLES, 0, strRI->vertexCnt); glexit("text drawing"); }
void handleInput(GameState* gs, InputState* is) { double te = gs->frameSpan; float moveSpeed = gs->settings.keyScroll * te; // should load from config float rotateSpeed = gs->settings.keyRotate * te; // 20.8 degrees float keyZoom = gs->settings.keyZoom * te; float mouseZoom = gs->settings.mouseZoom * te; if(is->clickButton == 1) { /* flattenArea(gs->map.tb, gs->cursorPos.x - 5, gs->cursorPos.y - 5, gs->cursorPos.x + 5, gs->cursorPos.y + 5 ); setZone(&gs->map, gs->cursorPos.x - 5, gs->cursorPos.y - 5, gs->cursorPos.x + 5, gs->cursorPos.y + 5, gs->activeTool + 1 ); checkMapDirty(&gs->map); */ } if(is->buttonDown == 1) { gs->mouseDownPos.x = gs->cursorPos.x; gs->mouseDownPos.y = gs->cursorPos.y; printf("start dragging at (%d,%d)\n", (int)gs->cursorPos.x, (int)gs->cursorPos.y); } if(is->buttonUp == 1) { //vCopy(&gs->cursorPos, &gs->mouseDownPos); printf("stopped dragging at (%d,%d)\n", (int)gs->cursorPos.x, (int)gs->cursorPos.y); } if(is->clickButton == 2) { gs->activeTool = (gs->activeTool + 1) % 3; } // look direction if(is->keyState[38] & IS_KEYDOWN) { gs->direction += rotateSpeed; } if(is->keyState[39] & IS_KEYDOWN) { gs->direction -= rotateSpeed; } // keep rotation in [0,F_2PI) gs->direction = fmodf(F_2PI + gs->direction, F_2PI); // zoom if(is->keyState[52] & IS_KEYDOWN) { gs->zoom += keyZoom; gs->zoom = fmin(gs->zoom, -10.0); } if(is->keyState[53] & IS_KEYDOWN) { gs->zoom -= keyZoom; } if(is->clickButton == 4) { gs->zoom += mouseZoom; gs->zoom = fmin(gs->zoom, -10.0); } if(is->clickButton == 5) { gs->zoom -= mouseZoom; } // movement Vector move = { .x = moveSpeed * sin(F_PI - gs->direction), .y = moveSpeed * cos(F_PI - gs->direction), .z = 0.0f }; if(is->keyState[111] & IS_KEYDOWN) { vAdd(&gs->lookCenter, &move, &gs->lookCenter); } if(is->keyState[116] & IS_KEYDOWN) { vSub(&gs->lookCenter, &move, &gs->lookCenter); } // flip x and y to get ccw normal, using move.z as the temp move.z = move.x; move.x = -move.y; move.y = move.z; move.z = 0.0f; if(is->keyState[113] & IS_KEYDOWN) { vSub(&gs->lookCenter, &move, &gs->lookCenter); } if(is->keyState[114] & IS_KEYDOWN) { vAdd(&gs->lookCenter, &move, &gs->lookCenter); } if(is->keyState[110] & IS_KEYDOWN) { nearPlane += 50 * te; printf("near: %f, far: %f\n", nearPlane, farPlane); } if(is->keyState[115] & IS_KEYDOWN) { nearPlane -= 50 * te; printf("near: %f, far: %f\n", nearPlane, farPlane); } if(is->keyState[112] & IS_KEYDOWN) { farPlane += 250 * te; printf("near: %f, far: %f\n", nearPlane, farPlane); } if(is->keyState[117] & IS_KEYDOWN) { farPlane -= 250 * te; printf("near: %f, far: %f\n", nearPlane, farPlane); } static lastChange = 0; if(is->keyState[119] & IS_KEYDOWN) { if(gs->frameTime > lastChange + 1) { gs->debugMode = (gs->debugMode + 1) % 5; lastChange = gs->frameTime; } } } void setGameSettings(GameSettings* g, UserConfig* u) { const float rotateFactor = 0.7260f; const float scrollFactor = 300.0f; const float zoomFactor = 600.0f; g->keyRotate = rotateFactor * fclampNorm(u->keyRotateSensitivity); g->keyScroll = scrollFactor * fclampNorm(u->keyScrollSensitivity); g->keyZoom = zoomFactor * fclampNorm(u->keyZoomSensitivity); g->mouseRotate = rotateFactor * fclampNorm(u->mouseRotateSensitivity); g->mouseScroll = scrollFactor * fclampNorm(u->mouseScrollSensitivity); g->mouseZoom = 4 * zoomFactor * fclampNorm(u->mouseZoomSensitivity); printf("keyRotate %.3f\n", g->keyRotate); printf("keyScroll %.3f\n", g->keyScroll); printf("keyZoom %.3f\n", g->keyZoom); printf("mouseRotate %.3f\n", g->mouseRotate); printf("mouseScroll %.3f\n", g->mouseScroll); printf("mouseZoom %.3f\n", g->mouseZoom); } void setUpView(GameState* gs) { } void depthPrepass(XStuff* xs, GameState* gs, InputState* is) { // draw UI renderUIPicking(xs, gs); // draw terrain // TODO: factor all the math into the frame setup function //mScale3f(10, 10, 10, &mModel); //mRot3f(0, 1, 0, gs->direction, &mModel); msPush(&gs->proj); msPerspective(60, gs->screen.aspect, nearPlane, farPlane, &gs->proj); msPush(&gs->view); // order matters! don't mess with this. msTrans3f(0, -1, gs->zoom, &gs->view); msRot3f(1, 0, 0, F_PI / 6, &gs->view); msRot3f(0,1,0, gs->direction, &gs->view); msTrans3f(-gs->lookCenter.x, 0, -gs->lookCenter.y, &gs->view); // y-up to z-up rotation msRot3f(1, 0, 0, F_PI_2, &gs->view); msScale3f(1, 1, -1, &gs->view); // calculate cursor position Vector eyeCoord; Vector worldCoord; Matrix p, invp, invv; // device space (-1:1) Vector devCoord; devCoord.x = 0.50; devCoord.y = 0.50; devCoord.z = -1.0; // eye space mInverse(msGetTop(&gs->proj), &invp); vMatrixMul(&devCoord, &invp, &eyeCoord); vNorm(&eyeCoord, &eyeCoord); // world space mInverse(msGetTop(&gs->view), &invv); vMatrixMul(&eyeCoord, &invv, &worldCoord); vNorm(&worldCoord, &worldCoord); // draw terrain // drawTerrainBlockDepth(&gs->map, msGetTop(&gs->model), msGetTop(&gs->view), msGetTop(&gs->proj)); drawTerrainDepth(&gs->map, msGetTop(&gs->view), msGetTop(&gs->proj), &gs->screen.wh); msPop(&gs->view); msPop(&gs->proj); }
bool hitRectan(Photon *photon, Rectan *rectan, double distance) { // Specular reflection if (true) { // Update ray's position to the point where the ray hits the sphere Vec vec = vMul(photon->direct, distance); photon->origin = vAdd(photon->origin, vec); // Angle between new and old vector is twice the angle between old vector and normal vec = vMul(rectan->direct, vDotp(photon->direct, rectan->direct) * 2.f); vec = vSub(rectan->direct, photon->direct); photon->direct = vNorm(vec); // Might not be necessary... vec = vMul(photon->direct, 1000.f * EPSILON); photon->origin = vAdd(photon->origin, vec); return REFLECTED; } // Diffuse reflection else if (false) { // Necessary for adjusting direction of reflected photon later on bool positiveAngle = NO; if (vDotp(photon->direct, rectan->direct) > EPSILON) positiveAngle = YES; // Update ray's position to the point where the ray hits the sphere Vec vec = vMul(photon->direct, distance); photon->origin = vAdd(photon->origin, vec); #warning DEBUG!!! // Camera origin /*Vec origin; origin.x = 0.99f; origin.y = 0.f; origin.z = 0.f; Vec newDir; newDir = vSub(&origin, &photon->origin); photon->direct = vNorm(&newDir);*/ // Using spherical coordinate system to guarantee // uniform distribution of orientations in space double azimuth = drand48() * M_PI * 2; // "phi" double inclination = acos(2 * drand48() - 1); // "theta" photon->direct.x = sin(inclination) * cos(azimuth); photon->direct.y = sin(inclination) * sin(azimuth); photon->direct.z = cos(inclination); // Adjusting direction of reflected photon // Incoming positive -> negative outgoing // Incomi ng negative -> positive outgoing if (positiveAngle) { if (vDotp(photon->direct, rectan->direct) > EPSILON) photon->direct = vMul(photon->direct, -1.f); } else if (vDotp(photon->direct, rectan->direct) < -EPSILON) photon->direct = vMul(photon->direct, -1.f); photon->direct = vNorm(photon->direct); // Might not be necessary... vec = vMul(photon->direct, 1000.f * EPSILON); photon->origin = vAdd(photon->origin, vec); // Color photon->wavelength = rectan->mID; return REFLECTED; } /* * Absorption */ else return ABSORBED; }
Triang newTriang(const Vec origin, const Vec vectab, const Vec vectac, const bool rectan, const uint mID) { return (Triang) { origin, vectab, vectac, vNorm(vCro(vectab, vectac)), rectan, mID }; }
void IMU::update() { // ======================================================================== // Accelerometer // Frame of reference: BODY // Units: G (gravitational acceleration) // Purpose: Measure the acceleration vector aVec with components // codirectional with the i, j, and k vectors. Note that the // gravitational vector is the negative of the K vector. // ======================================================================== #ifdef ACC_WEIGHT if (loopCount % ACC_READ_INTERVAL == 0) { acc.poll(); for (int i=0; i<3; i++) { aVec[i] = acc.get(i); } // Take weighted average. #ifdef ACC_SELF_WEIGHT for (int i=0; i<3; i++) { aVec[i] = ACC_SELF_WEIGHT * aVec[i] + (1-ACC_SELF_WEIGHT) * aVecLast[i]; aVecLast[i] = aVec[i]; // Kalman filtering? //aVecLast[i] = acc.get(i); //kalmanUpdate(aVec[i], accVar, aVecLast[i], ACC_UPDATE_SIG); //kalmanPredict(aVec[i], accVar, 0.0, ACC_PREDICT_SIG); } #endif // ACC_SELF_WEIGHT accScale = vNorm(aVec); // Reduce accelerometer weight if the magnitude of the measured // acceleration is significantly greater than or less than 1 g. // // TODO: Magnitude of acceleration should be reported over telemetry so // the "cutoff" value (the constant before the ABS() below) for // disregaring acceleration input can be more accurately determined. #ifdef ACC_SCALE_WEIGHT // For some reason, 1 gravity has magnitude of 1.05. accScale = (1.0 - MIN(1.0, ACC_SCALE_WEIGHT * ABS(accScale - 1.0))); accWeight = ACC_WEIGHT * accScale; #else accWeight = ACC_WEIGHT; // Ignore accelerometer if it measures anything 0.5g past gravity. if (accScale > 1.5 || accScale < 0.5) { accWeight = 0; } #endif // ACC_SCALE_WEIGHT // Uncomment the loop below to get accelerometer readings in order to // obtain trimAngle. //if (loopCount % COMM_LOOP_INTERVAL == 0) { // sp("A("); // sp(aVec[0]*1000); sp(", "); // sp(aVec[1]*1000); sp(", "); // sp(aVec[2]*1000); // sp(") "); //} // Express K global unit vector in BODY frame as kgb for use in drift // correction (we need K to be described in the BODY frame because // gravity is measured by the accelerometer in the BODY frame). // Technically we could just create a transpose of gyroDCM, but since // we don't (yet) have a magnetometer, we don't need the first two rows // of the transpose. This saves a few clock cycles. for (int i=0; i<3; i++) { kgb[i] = gyroDCM[i][2]; } // Calculate gyro drift correction rotation vector wA, which will be // used later to bring KB closer to the gravity vector (i.e., the // negative of the K vector). Although we do not explicitly negate the // gravity vector, the cross product below produces a rotation vector // that we can later add to the angular displacement vector to correct // for gyro drift in the X and Y axes. vCrossP(kgb, aVec, wA); // Divide by ACC_READ_INTERVAL since the acceleration vector is not // updated every loop. for (int i=0; i<3; i++) { wA[i] /= ACC_READ_INTERVAL; } } #endif // ACC_WEIGHT // ======================================================================== // Magnetometer // Frame of reference: BODY // Units: N/A // Purpose: Measure the magnetic north vector mVec with components // codirectional with the body's i, j, and k vectors. // ======================================================================== #ifdef MAG_WEIGHT mag.poll(); // ? for (int i=0; i<3; i++) { mVec[i] = mag.get(i); } //if (loopCount % COMM_LOOP_INTERVAL == 0) { // sp("M("); // sp(mVec[0]); sp(", "); // sp(mVec[1]); sp(", "); // sp(mVec[2]); // spln(")"); //} // Express J global unit vectory in BODY frame as jgb. for (int i=0; i<3; i++) { jgb[i] = gyroDCM[i][1]; } // Calculate yaw drift correction vector wM. vCrossP(jgb, mVec, wM); #endif // MAG_WEIGHT // ======================================================================== // Gyroscope // Frame of reference: BODY // Units: rad/s // Purpose: Measure the rotation rate of the body about the body's i, // j, and k axes. // ======================================================================== gyro.poll(); for (int i=0; i<3; i++) { gVec[i] = gyro.get(i); } //if (loopCount % COMM_LOOP_INTERVAL == 0) { // sp("G("); // sp(gVec[0]); sp(", "); // sp(gVec[1]); sp(", "); // sp(gVec[2]); // spln(")"); //} // Scale gVec by elapsed time (in seconds) to get angle w*dt in // radians, then compute weighted average with the accelerometer and // magnetometer correction vectors to obtain final w*dt. for (int i=0; i<3; i++) { float numerator = gVec[i] * MASTER_DT/1000000; float denominator = 1.0; #ifdef ACC_WEIGHT if (loopCount % ACC_READ_INTERVAL == 0) { numerator += accWeight * wA[i]; denominator += accWeight; } #endif // ACC_WEIGHT #ifdef MAG_WEIGHT numerator += MAG_WEIGHT * wM[i]; denominator += MAG_WEIGHT; #endif // MAG_WEIGHT wdt[i] = numerator / denominator; } // ======================================================================== // Direction Cosine Matrix // Frame of reference: GLOBAL // Units: None (unit vectors) // Purpose: Calculate the components of the body's i, j, and k unit // vectors in the global frame of reference. // ======================================================================== // Skew the rotation vector and sum appropriate components by combining the // skew symmetric matrix with the identity matrix. The math can be // summarized as follows: // // All of this is calculated in the BODY frame. If w is the angular // velocity vector, let wdt (w*dt) be the angular displacement vector of // the DCM over a time interval dt. Let wdt_i, wdt_j, and wdt_k be the // components of wdt codirectional with the i, j, and k unit vectors, // respectively. Also, let dr be the linear displacement vector of the DCM // and dr_i, dr_j, and dr_k once again be the i, j, and k components, // respectively. // // In very small dt, certain vectors approach orthogonality, so we can // assume that (draw this out for yourself!): // // dr_x = < 0, dw_k, -dw_j>, // dr_y = <-dw_k, 0, dw_i>, and // dr_z = < dw_j, -dw_i, 0>, // // which can be expressed as the rotation matrix: // // [ 0 dw_k -dw_j ] // dr = [ -dw_k 0 dw_i ] // [ dw_j -dw_i 0 ]. // // This can then be multiplied by the current DCM and added to the current // DCM to update the DCM. To minimize the number of calculations performed // by the processor, however, we can combine the last two steps by // combining dr with the identity matrix to produce: // // [ 1 dw_k -dw_j ] // dr + I = [ -dw_k 1 dw_i ] // [ dw_j -dw_i 1 ], // // which we multiply with the current DCM to produce the updated DCM // directly. // // It may be helpful to read the Wikipedia pages on cross products and // rotation representation. // ======================================================================== dDCM[0][0] = 1; dDCM[0][1] = wdt[2]; dDCM[0][2] = -wdt[1]; dDCM[1][0] = -wdt[2]; dDCM[1][1] = 1; dDCM[1][2] = wdt[0]; dDCM[2][0] = wdt[1]; dDCM[2][1] = -wdt[0]; dDCM[2][2] = 1; // Multiply the current DCM with the change in DCM and update. mProduct(dDCM, gyroDCM, gyroDCM); orthonormalize(gyroDCM); #ifdef ACC_WEIGHT trimDCM[0][0] = 1; trimDCM[0][1] = 0; trimDCM[0][2] = -trimAngle[1]; trimDCM[1][0] = 0; trimDCM[1][1] = 1; trimDCM[1][2] = trimAngle[0]; trimDCM[2][0] = trimAngle[1]; trimDCM[2][1] = -trimAngle[0]; trimDCM[2][2] = 1; // Rotate gyroDCM with trimDCM. mProduct(trimDCM, gyroDCM, bodyDCM); //orthonormalize(bodyDCM); // TODO: This shouldn't be necessary. #else for (int i=0; i<3; i++) { for (int j=0; j<3; j++) { bodyDCM[i][j] = gyroDCM[i][j]; } } #endif // ACC_WEIGHT }