bool AABBox::intersectRay(const Ray& ray, float& distance) const {
//TODO: this can be precomputed
//the inverse of the values in the ray's direction vector
Vector3 inverseDirection(1.0f / ray.direction().x(),
1.0f / ray.direction().y(),
1.0f / ray.direction().z());
//find the intersection with the closest and farthest lines on all three axes
float tx1 = (m_lowerCorner.x() - ray.origin().x()) * inverseDirection.x();
float tx2 = (m_upperCorner.x() - ray.origin().x()) * inverseDirection.x();
float ty1 = (m_lowerCorner.y() - ray.origin().y()) * inverseDirection.y();
float ty2 = (m_upperCorner.y() - ray.origin().y()) * inverseDirection.y();
float tz1 = (m_lowerCorner.z() - ray.origin().z()) * inverseDirection.z();
float tz2 = (m_upperCorner.z() - ray.origin().z()) * inverseDirection.z();
//the highest closer intersection
float tNear(fmax(fmax(fmin(tx1, tx2), fmin(ty1, ty2)), fmin(tz1, tz2)));
//the lowest farther intersection
float tFar(fmin(fmin(fmax(tx1, tx2), fmax(ty1, ty2)), fmax(tz1, tz2)));
//missed
if (tNear > tFar) {
return false;
}
//behind ray
if (tFar < 0) {
return false;
}
//distance of box is more than max distance
if (tNear > distance) {
return false;
}
return true;
}
static void addMeasurement(Packet_t *packet)
{
xvector[vectorPos] = packet->accX;
yvector[vectorPos] = packet->accY;
zvector[vectorPos] = packet->accZ;
vectorPos++;
if (vectorPos < NUM_SAMPLES) return;
vectorPos = 0;
float s = stdev(xvector) + stdev(yvector) + stdev(zvector);
// PRINTF("stdev = %u\n", (uint16_t) s);
float accelVector[3] = {
average(xvector) - groundVector[0],
average(yvector) - groundVector[1],
average(zvector) - groundVector[2]
};
// PRINTF("accel=(%d %d %d)\n", (int)accelVector[0], (int)accelVector[1], (int)accelVector[2]);
bool nowFwd, nowBack;
float accLen = vlength(accelVector);
if (accLen < ONE_G / 10) {
nowBack = nowFwd = false;
} else {
normalizeQuick(accelVector, accLen);
nowFwd = sameDirection(fwdVector, accelVector);
nowBack = inverseDirection(fwdVector, accelVector);
}
int now = nowFwd ? 1 : (nowBack ? -1 : 0);
bool diffFromPrevious;
static bool notFirstTime;
if (notFirstTime) {
// float difference = (calcDiff(xvector, prevxvector)
// + calcDiff(yvector, prevyvector)
// + calcDiff(zvector, prevzvector)) / 3.0;
float difference = calcDiff(xvector, prevavgxvector)
+ calcDiff(yvector, prevavgyvector)
+ calcDiff(zvector, prevavgzvector);
// PRINTF("difference=%d\n", (int)difference);
diffFromPrevious = (difference >= 30 * (ONE_G / 10));
} else {
notFirstTime = true;
diffFromPrevious = false;
}
bool pastBack = false;
bool pastFwd = false;
if (past[0] + past[1] + past[2] >= 1) {
pastFwd = true;
} else if (past[0] + past[1] + past[2] <= -1) {
pastBack = true;
}
past[0] = past[1];
past[1] = past[2];
past[2] = now;
bool wasMoving = !bayesStandingMode;
#if 0
PRINTF("SSD \t%d\n", (int)(s >= THRESH_LOW));
PRINTF("MSD \t%d\n", (int)(s >= THRESH_MEDIUM));
PRINTF("LSD \t%d\n" , (int)(s > THRESH_HIGH));
PRINTF("Accelerating\t%d\n", (int)(nowFwd));
PRINTF("PastAccel \t%d\n", (int)(pastFwd));
PRINTF("PastBrake \t%d\n", (int)(pastBack));
PRINTF("WasMoving \t%d\n", (int)(wasMoving));
PRINTF("Diff \t%d\n", (int)(diffFromPrevious));
#endif
calcNewProb(s, wasMoving, pastBack, pastFwd, nowFwd, diffFromPrevious);
}
