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