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
}
