am 00ff4748: Merge "More VelocityTracker refactoring." into jb-dev
* commit '00ff47484f8137aa3e59f680ff07d2662cfb4088': More VelocityTracker refactoring.
This commit is contained in:
@@ -60,8 +60,7 @@ public final class VelocityTracker implements Poolable<VelocityTracker> {
|
|||||||
private static native void nativeComputeCurrentVelocity(int ptr, int units, float maxVelocity);
|
private static native void nativeComputeCurrentVelocity(int ptr, int units, float maxVelocity);
|
||||||
private static native float nativeGetXVelocity(int ptr, int id);
|
private static native float nativeGetXVelocity(int ptr, int id);
|
||||||
private static native float nativeGetYVelocity(int ptr, int id);
|
private static native float nativeGetYVelocity(int ptr, int id);
|
||||||
private static native boolean nativeGetEstimator(int ptr, int id,
|
private static native boolean nativeGetEstimator(int ptr, int id, Estimator outEstimator);
|
||||||
int degree, int horizonMillis, Estimator outEstimator);
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Retrieve a new VelocityTracker object to watch the velocity of a
|
* Retrieve a new VelocityTracker object to watch the velocity of a
|
||||||
@@ -227,21 +226,17 @@ public final class VelocityTracker implements Poolable<VelocityTracker> {
|
|||||||
* this method.
|
* this method.
|
||||||
*
|
*
|
||||||
* @param id Which pointer's velocity to return.
|
* @param id Which pointer's velocity to return.
|
||||||
* @param degree The desired polynomial degree. The actual estimator may have
|
|
||||||
* a lower degree than what is requested here. If -1, uses the default degree.
|
|
||||||
* @param horizonMillis The maximum age of the oldest sample to consider, in milliseconds.
|
|
||||||
* If -1, uses the default horizon.
|
|
||||||
* @param outEstimator The estimator to populate.
|
* @param outEstimator The estimator to populate.
|
||||||
* @return True if an estimator was obtained, false if there is no information
|
* @return True if an estimator was obtained, false if there is no information
|
||||||
* available about the pointer.
|
* available about the pointer.
|
||||||
*
|
*
|
||||||
* @hide For internal use only. Not a final API.
|
* @hide For internal use only. Not a final API.
|
||||||
*/
|
*/
|
||||||
public boolean getEstimator(int id, int degree, int horizonMillis, Estimator outEstimator) {
|
public boolean getEstimator(int id, Estimator outEstimator) {
|
||||||
if (outEstimator == null) {
|
if (outEstimator == null) {
|
||||||
throw new IllegalArgumentException("outEstimator must not be null");
|
throw new IllegalArgumentException("outEstimator must not be null");
|
||||||
}
|
}
|
||||||
return nativeGetEstimator(mPtr, id, degree, horizonMillis, outEstimator);
|
return nativeGetEstimator(mPtr, id, outEstimator);
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
|||||||
@@ -527,7 +527,7 @@ public class PointerLocationView extends View implements InputDeviceListener {
|
|||||||
ps.addTrace(coords.x, coords.y);
|
ps.addTrace(coords.x, coords.y);
|
||||||
ps.mXVelocity = mVelocity.getXVelocity(id);
|
ps.mXVelocity = mVelocity.getXVelocity(id);
|
||||||
ps.mYVelocity = mVelocity.getYVelocity(id);
|
ps.mYVelocity = mVelocity.getYVelocity(id);
|
||||||
mVelocity.getEstimator(id, -1, -1, ps.mEstimator);
|
mVelocity.getEstimator(id, ps.mEstimator);
|
||||||
ps.mToolType = event.getToolType(i);
|
ps.mToolType = event.getToolType(i);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -48,8 +48,7 @@ public:
|
|||||||
void addMovement(const MotionEvent* event);
|
void addMovement(const MotionEvent* event);
|
||||||
void computeCurrentVelocity(int32_t units, float maxVelocity);
|
void computeCurrentVelocity(int32_t units, float maxVelocity);
|
||||||
void getVelocity(int32_t id, float* outVx, float* outVy);
|
void getVelocity(int32_t id, float* outVx, float* outVy);
|
||||||
bool getEstimator(int32_t id, uint32_t degree, nsecs_t horizon,
|
bool getEstimator(int32_t id, VelocityTracker::Estimator* outEstimator);
|
||||||
VelocityTracker::Estimator* outEstimator);
|
|
||||||
|
|
||||||
private:
|
private:
|
||||||
struct Velocity {
|
struct Velocity {
|
||||||
@@ -129,9 +128,8 @@ void VelocityTrackerState::getVelocity(int32_t id, float* outVx, float* outVy) {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
bool VelocityTrackerState::getEstimator(int32_t id, uint32_t degree, nsecs_t horizon,
|
bool VelocityTrackerState::getEstimator(int32_t id, VelocityTracker::Estimator* outEstimator) {
|
||||||
VelocityTracker::Estimator* outEstimator) {
|
return mVelocityTracker.getEstimator(id, outEstimator);
|
||||||
return mVelocityTracker.getEstimator(id, degree, horizon, outEstimator);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
@@ -186,14 +184,10 @@ static jfloat android_view_VelocityTracker_nativeGetYVelocity(JNIEnv* env, jclas
|
|||||||
}
|
}
|
||||||
|
|
||||||
static jboolean android_view_VelocityTracker_nativeGetEstimator(JNIEnv* env, jclass clazz,
|
static jboolean android_view_VelocityTracker_nativeGetEstimator(JNIEnv* env, jclass clazz,
|
||||||
jint ptr, jint id, jint degree, jint horizonMillis, jobject outEstimatorObj) {
|
jint ptr, jint id, jobject outEstimatorObj) {
|
||||||
VelocityTrackerState* state = reinterpret_cast<VelocityTrackerState*>(ptr);
|
VelocityTrackerState* state = reinterpret_cast<VelocityTrackerState*>(ptr);
|
||||||
VelocityTracker::Estimator estimator;
|
VelocityTracker::Estimator estimator;
|
||||||
bool result = state->getEstimator(id,
|
bool result = state->getEstimator(id, &estimator);
|
||||||
degree < 0 ? VelocityTracker::DEFAULT_DEGREE : uint32_t(degree),
|
|
||||||
horizonMillis < 0 ? VelocityTracker::DEFAULT_HORIZON :
|
|
||||||
nsecs_t(horizonMillis) * 1000000L,
|
|
||||||
&estimator);
|
|
||||||
|
|
||||||
jfloatArray xCoeffObj = jfloatArray(env->GetObjectField(outEstimatorObj,
|
jfloatArray xCoeffObj = jfloatArray(env->GetObjectField(outEstimatorObj,
|
||||||
gEstimatorClassInfo.xCoeff));
|
gEstimatorClassInfo.xCoeff));
|
||||||
@@ -236,7 +230,7 @@ static JNINativeMethod gVelocityTrackerMethods[] = {
|
|||||||
"(II)F",
|
"(II)F",
|
||||||
(void*)android_view_VelocityTracker_nativeGetYVelocity },
|
(void*)android_view_VelocityTracker_nativeGetYVelocity },
|
||||||
{ "nativeGetEstimator",
|
{ "nativeGetEstimator",
|
||||||
"(IIIILandroid/view/VelocityTracker$Estimator;)Z",
|
"(IILandroid/view/VelocityTracker$Estimator;)Z",
|
||||||
(void*)android_view_VelocityTracker_nativeGetEstimator },
|
(void*)android_view_VelocityTracker_nativeGetEstimator },
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|||||||
@@ -23,19 +23,13 @@
|
|||||||
|
|
||||||
namespace android {
|
namespace android {
|
||||||
|
|
||||||
|
class VelocityTrackerStrategy;
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Calculates the velocity of pointer movements over time.
|
* Calculates the velocity of pointer movements over time.
|
||||||
*/
|
*/
|
||||||
class VelocityTracker {
|
class VelocityTracker {
|
||||||
public:
|
public:
|
||||||
// Default polynomial degree. (used by getVelocity)
|
|
||||||
static const uint32_t DEFAULT_DEGREE = 2;
|
|
||||||
|
|
||||||
// Default sample horizon. (used by getVelocity)
|
|
||||||
// We don't use too much history by default since we want to react to quick
|
|
||||||
// changes in direction.
|
|
||||||
static const nsecs_t DEFAULT_HORIZON = 100 * 1000000; // 100 ms
|
|
||||||
|
|
||||||
struct Position {
|
struct Position {
|
||||||
float x, y;
|
float x, y;
|
||||||
};
|
};
|
||||||
@@ -64,6 +58,8 @@ public:
|
|||||||
};
|
};
|
||||||
|
|
||||||
VelocityTracker();
|
VelocityTracker();
|
||||||
|
VelocityTracker(VelocityTrackerStrategy* strategy);
|
||||||
|
~VelocityTracker();
|
||||||
|
|
||||||
// Resets the velocity tracker state.
|
// Resets the velocity tracker state.
|
||||||
void clear();
|
void clear();
|
||||||
@@ -88,35 +84,80 @@ public:
|
|||||||
// insufficient movement information for the pointer.
|
// insufficient movement information for the pointer.
|
||||||
bool getVelocity(uint32_t id, float* outVx, float* outVy) const;
|
bool getVelocity(uint32_t id, float* outVx, float* outVy) const;
|
||||||
|
|
||||||
// Gets a quadratic estimator for the movements of the specified pointer id.
|
// Gets an estimator for the recent movements of the specified pointer id.
|
||||||
// Returns false and clears the estimator if there is no information available
|
// Returns false and clears the estimator if there is no information available
|
||||||
// about the pointer.
|
// about the pointer.
|
||||||
bool getEstimator(uint32_t id, uint32_t degree, nsecs_t horizon,
|
bool getEstimator(uint32_t id, Estimator* outEstimator) const;
|
||||||
Estimator* outEstimator) const;
|
|
||||||
|
|
||||||
// Gets the active pointer id, or -1 if none.
|
// Gets the active pointer id, or -1 if none.
|
||||||
inline int32_t getActivePointerId() const { return mActivePointerId; }
|
inline int32_t getActivePointerId() const { return mActivePointerId; }
|
||||||
|
|
||||||
// Gets a bitset containing all pointer ids from the most recent movement.
|
// Gets a bitset containing all pointer ids from the most recent movement.
|
||||||
inline BitSet32 getCurrentPointerIdBits() const { return mMovements[mIndex].idBits; }
|
inline BitSet32 getCurrentPointerIdBits() const { return mCurrentPointerIdBits; }
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
BitSet32 mCurrentPointerIdBits;
|
||||||
|
int32_t mActivePointerId;
|
||||||
|
VelocityTrackerStrategy* mStrategy;
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Implements a particular velocity tracker algorithm.
|
||||||
|
*/
|
||||||
|
class VelocityTrackerStrategy {
|
||||||
|
protected:
|
||||||
|
VelocityTrackerStrategy() { }
|
||||||
|
|
||||||
|
public:
|
||||||
|
virtual ~VelocityTrackerStrategy() { }
|
||||||
|
|
||||||
|
virtual void clear() = 0;
|
||||||
|
virtual void clearPointers(BitSet32 idBits) = 0;
|
||||||
|
virtual void addMovement(nsecs_t eventTime, BitSet32 idBits,
|
||||||
|
const VelocityTracker::Position* positions) = 0;
|
||||||
|
virtual bool getEstimator(uint32_t id, VelocityTracker::Estimator* outEstimator) const = 0;
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Velocity tracker algorithm based on least-squares linear regression.
|
||||||
|
*/
|
||||||
|
class LeastSquaresVelocityTrackerStrategy : public VelocityTrackerStrategy {
|
||||||
|
public:
|
||||||
|
LeastSquaresVelocityTrackerStrategy();
|
||||||
|
virtual ~LeastSquaresVelocityTrackerStrategy();
|
||||||
|
|
||||||
|
virtual void clear();
|
||||||
|
virtual void clearPointers(BitSet32 idBits);
|
||||||
|
virtual void addMovement(nsecs_t eventTime, BitSet32 idBits,
|
||||||
|
const VelocityTracker::Position* positions);
|
||||||
|
virtual bool getEstimator(uint32_t id, VelocityTracker::Estimator* outEstimator) const;
|
||||||
|
|
||||||
|
private:
|
||||||
|
// Polynomial degree. Must be less than or equal to Estimator::MAX_DEGREE.
|
||||||
|
static const uint32_t DEGREE = 2;
|
||||||
|
|
||||||
|
// Sample horizon.
|
||||||
|
// We don't use too much history by default since we want to react to quick
|
||||||
|
// changes in direction.
|
||||||
|
static const nsecs_t HORIZON = 100 * 1000000; // 100 ms
|
||||||
|
|
||||||
// Number of samples to keep.
|
// Number of samples to keep.
|
||||||
static const uint32_t HISTORY_SIZE = 20;
|
static const uint32_t HISTORY_SIZE = 20;
|
||||||
|
|
||||||
struct Movement {
|
struct Movement {
|
||||||
nsecs_t eventTime;
|
nsecs_t eventTime;
|
||||||
BitSet32 idBits;
|
BitSet32 idBits;
|
||||||
Position positions[MAX_POINTERS];
|
VelocityTracker::Position positions[MAX_POINTERS];
|
||||||
|
|
||||||
inline const Position& getPosition(uint32_t id) const {
|
inline const VelocityTracker::Position& getPosition(uint32_t id) const {
|
||||||
return positions[idBits.getIndexOfBit(id)];
|
return positions[idBits.getIndexOfBit(id)];
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
uint32_t mIndex;
|
uint32_t mIndex;
|
||||||
Movement mMovements[HISTORY_SIZE];
|
Movement mMovements[HISTORY_SIZE];
|
||||||
int32_t mActivePointerId;
|
|
||||||
};
|
};
|
||||||
|
|
||||||
} // namespace android
|
} // namespace android
|
||||||
|
|||||||
@@ -33,13 +33,7 @@
|
|||||||
|
|
||||||
namespace android {
|
namespace android {
|
||||||
|
|
||||||
// --- VelocityTracker ---
|
static float vectorDot(const float* a, const float* b, uint32_t m) {
|
||||||
|
|
||||||
const uint32_t VelocityTracker::DEFAULT_DEGREE;
|
|
||||||
const nsecs_t VelocityTracker::DEFAULT_HORIZON;
|
|
||||||
const uint32_t VelocityTracker::HISTORY_SIZE;
|
|
||||||
|
|
||||||
static inline float vectorDot(const float* a, const float* b, uint32_t m) {
|
|
||||||
float r = 0;
|
float r = 0;
|
||||||
while (m--) {
|
while (m--) {
|
||||||
r += *(a++) * *(b++);
|
r += *(a++) * *(b++);
|
||||||
@@ -47,7 +41,7 @@ static inline float vectorDot(const float* a, const float* b, uint32_t m) {
|
|||||||
return r;
|
return r;
|
||||||
}
|
}
|
||||||
|
|
||||||
static inline float vectorNorm(const float* a, uint32_t m) {
|
static float vectorNorm(const float* a, uint32_t m) {
|
||||||
float r = 0;
|
float r = 0;
|
||||||
while (m--) {
|
while (m--) {
|
||||||
float t = *(a++);
|
float t = *(a++);
|
||||||
@@ -91,45 +85,52 @@ static String8 matrixToString(const float* a, uint32_t m, uint32_t n, bool rowMa
|
|||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
VelocityTracker::VelocityTracker() {
|
|
||||||
clear();
|
// --- VelocityTracker ---
|
||||||
|
|
||||||
|
VelocityTracker::VelocityTracker() :
|
||||||
|
mCurrentPointerIdBits(0), mActivePointerId(-1),
|
||||||
|
mStrategy(new LeastSquaresVelocityTrackerStrategy()) {
|
||||||
|
}
|
||||||
|
|
||||||
|
VelocityTracker::VelocityTracker(VelocityTrackerStrategy* strategy) :
|
||||||
|
mCurrentPointerIdBits(0), mActivePointerId(-1),
|
||||||
|
mStrategy(strategy) {
|
||||||
|
}
|
||||||
|
|
||||||
|
VelocityTracker::~VelocityTracker() {
|
||||||
|
delete mStrategy;
|
||||||
}
|
}
|
||||||
|
|
||||||
void VelocityTracker::clear() {
|
void VelocityTracker::clear() {
|
||||||
mIndex = 0;
|
mCurrentPointerIdBits.clear();
|
||||||
mMovements[0].idBits.clear();
|
|
||||||
mActivePointerId = -1;
|
mActivePointerId = -1;
|
||||||
|
|
||||||
|
mStrategy->clear();
|
||||||
}
|
}
|
||||||
|
|
||||||
void VelocityTracker::clearPointers(BitSet32 idBits) {
|
void VelocityTracker::clearPointers(BitSet32 idBits) {
|
||||||
BitSet32 remainingIdBits(mMovements[mIndex].idBits.value & ~idBits.value);
|
BitSet32 remainingIdBits(mCurrentPointerIdBits.value & ~idBits.value);
|
||||||
mMovements[mIndex].idBits = remainingIdBits;
|
mCurrentPointerIdBits = remainingIdBits;
|
||||||
|
|
||||||
if (mActivePointerId >= 0 && idBits.hasBit(mActivePointerId)) {
|
if (mActivePointerId >= 0 && idBits.hasBit(mActivePointerId)) {
|
||||||
mActivePointerId = !remainingIdBits.isEmpty() ? remainingIdBits.firstMarkedBit() : -1;
|
mActivePointerId = !remainingIdBits.isEmpty() ? remainingIdBits.firstMarkedBit() : -1;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
mStrategy->clearPointers(idBits);
|
||||||
}
|
}
|
||||||
|
|
||||||
void VelocityTracker::addMovement(nsecs_t eventTime, BitSet32 idBits, const Position* positions) {
|
void VelocityTracker::addMovement(nsecs_t eventTime, BitSet32 idBits, const Position* positions) {
|
||||||
if (++mIndex == HISTORY_SIZE) {
|
|
||||||
mIndex = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
while (idBits.count() > MAX_POINTERS) {
|
while (idBits.count() > MAX_POINTERS) {
|
||||||
idBits.clearLastMarkedBit();
|
idBits.clearLastMarkedBit();
|
||||||
}
|
}
|
||||||
|
|
||||||
Movement& movement = mMovements[mIndex];
|
mCurrentPointerIdBits = idBits;
|
||||||
movement.eventTime = eventTime;
|
if (mActivePointerId < 0 || !idBits.hasBit(mActivePointerId)) {
|
||||||
movement.idBits = idBits;
|
mActivePointerId = idBits.isEmpty() ? -1 : idBits.firstMarkedBit();
|
||||||
uint32_t count = idBits.count();
|
|
||||||
for (uint32_t i = 0; i < count; i++) {
|
|
||||||
movement.positions[i] = positions[i];
|
|
||||||
}
|
}
|
||||||
|
|
||||||
if (mActivePointerId < 0 || !idBits.hasBit(mActivePointerId)) {
|
mStrategy->addMovement(eventTime, idBits, positions);
|
||||||
mActivePointerId = count != 0 ? idBits.firstMarkedBit() : -1;
|
|
||||||
}
|
|
||||||
|
|
||||||
#if DEBUG_VELOCITY
|
#if DEBUG_VELOCITY
|
||||||
ALOGD("VelocityTracker: addMovement eventTime=%lld, idBits=0x%08x, activePointerId=%d",
|
ALOGD("VelocityTracker: addMovement eventTime=%lld, idBits=0x%08x, activePointerId=%d",
|
||||||
@@ -139,7 +140,7 @@ void VelocityTracker::addMovement(nsecs_t eventTime, BitSet32 idBits, const Posi
|
|||||||
uint32_t index = idBits.getIndexOfBit(id);
|
uint32_t index = idBits.getIndexOfBit(id);
|
||||||
iterBits.clearBit(id);
|
iterBits.clearBit(id);
|
||||||
Estimator estimator;
|
Estimator estimator;
|
||||||
getEstimator(id, DEFAULT_DEGREE, DEFAULT_HORIZON, &estimator);
|
getEstimator(id, &estimator);
|
||||||
ALOGD(" %d: position (%0.3f, %0.3f), "
|
ALOGD(" %d: position (%0.3f, %0.3f), "
|
||||||
"estimator (degree=%d, xCoeff=%s, yCoeff=%s, confidence=%f)",
|
"estimator (degree=%d, xCoeff=%s, yCoeff=%s, confidence=%f)",
|
||||||
id, positions[index].x, positions[index].y,
|
id, positions[index].x, positions[index].y,
|
||||||
@@ -215,6 +216,61 @@ void VelocityTracker::addMovement(const MotionEvent* event) {
|
|||||||
addMovement(eventTime, idBits, positions);
|
addMovement(eventTime, idBits, positions);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
bool VelocityTracker::getVelocity(uint32_t id, float* outVx, float* outVy) const {
|
||||||
|
Estimator estimator;
|
||||||
|
if (getEstimator(id, &estimator) && estimator.degree >= 1) {
|
||||||
|
*outVx = estimator.xCoeff[1];
|
||||||
|
*outVy = estimator.yCoeff[1];
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
*outVx = 0;
|
||||||
|
*outVy = 0;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool VelocityTracker::getEstimator(uint32_t id, Estimator* outEstimator) const {
|
||||||
|
return mStrategy->getEstimator(id, outEstimator);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// --- LeastSquaresVelocityTrackerStrategy ---
|
||||||
|
|
||||||
|
const uint32_t LeastSquaresVelocityTrackerStrategy::DEGREE;
|
||||||
|
const nsecs_t LeastSquaresVelocityTrackerStrategy::HORIZON;
|
||||||
|
const uint32_t LeastSquaresVelocityTrackerStrategy::HISTORY_SIZE;
|
||||||
|
|
||||||
|
LeastSquaresVelocityTrackerStrategy::LeastSquaresVelocityTrackerStrategy() {
|
||||||
|
clear();
|
||||||
|
}
|
||||||
|
|
||||||
|
LeastSquaresVelocityTrackerStrategy::~LeastSquaresVelocityTrackerStrategy() {
|
||||||
|
}
|
||||||
|
|
||||||
|
void LeastSquaresVelocityTrackerStrategy::clear() {
|
||||||
|
mIndex = 0;
|
||||||
|
mMovements[0].idBits.clear();
|
||||||
|
}
|
||||||
|
|
||||||
|
void LeastSquaresVelocityTrackerStrategy::clearPointers(BitSet32 idBits) {
|
||||||
|
BitSet32 remainingIdBits(mMovements[mIndex].idBits.value & ~idBits.value);
|
||||||
|
mMovements[mIndex].idBits = remainingIdBits;
|
||||||
|
}
|
||||||
|
|
||||||
|
void LeastSquaresVelocityTrackerStrategy::addMovement(nsecs_t eventTime, BitSet32 idBits,
|
||||||
|
const VelocityTracker::Position* positions) {
|
||||||
|
if (++mIndex == HISTORY_SIZE) {
|
||||||
|
mIndex = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
Movement& movement = mMovements[mIndex];
|
||||||
|
movement.eventTime = eventTime;
|
||||||
|
movement.idBits = idBits;
|
||||||
|
uint32_t count = idBits.count();
|
||||||
|
for (uint32_t i = 0; i < count; i++) {
|
||||||
|
movement.positions[i] = positions[i];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Solves a linear least squares problem to obtain a N degree polynomial that fits
|
* Solves a linear least squares problem to obtain a N degree polynomial that fits
|
||||||
* the specified input data as nearly as possible.
|
* the specified input data as nearly as possible.
|
||||||
@@ -361,22 +417,8 @@ static bool solveLeastSquares(const float* x, const float* y, uint32_t m, uint32
|
|||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool VelocityTracker::getVelocity(uint32_t id, float* outVx, float* outVy) const {
|
bool LeastSquaresVelocityTrackerStrategy::getEstimator(uint32_t id,
|
||||||
Estimator estimator;
|
VelocityTracker::Estimator* outEstimator) const {
|
||||||
if (getEstimator(id, DEFAULT_DEGREE, DEFAULT_HORIZON, &estimator)) {
|
|
||||||
if (estimator.degree >= 1) {
|
|
||||||
*outVx = estimator.xCoeff[1];
|
|
||||||
*outVy = estimator.yCoeff[1];
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
*outVx = 0;
|
|
||||||
*outVy = 0;
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool VelocityTracker::getEstimator(uint32_t id, uint32_t degree, nsecs_t horizon,
|
|
||||||
Estimator* outEstimator) const {
|
|
||||||
outEstimator->clear();
|
outEstimator->clear();
|
||||||
|
|
||||||
// Iterate over movement samples in reverse time order and collect samples.
|
// Iterate over movement samples in reverse time order and collect samples.
|
||||||
@@ -393,11 +435,11 @@ bool VelocityTracker::getEstimator(uint32_t id, uint32_t degree, nsecs_t horizon
|
|||||||
}
|
}
|
||||||
|
|
||||||
nsecs_t age = newestMovement.eventTime - movement.eventTime;
|
nsecs_t age = newestMovement.eventTime - movement.eventTime;
|
||||||
if (age > horizon) {
|
if (age > HORIZON) {
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
const Position& position = movement.getPosition(id);
|
const VelocityTracker::Position& position = movement.getPosition(id);
|
||||||
x[m] = position.x;
|
x[m] = position.x;
|
||||||
y[m] = position.y;
|
y[m] = position.y;
|
||||||
time[m] = -age * 0.000000001f;
|
time[m] = -age * 0.000000001f;
|
||||||
@@ -409,9 +451,7 @@ bool VelocityTracker::getEstimator(uint32_t id, uint32_t degree, nsecs_t horizon
|
|||||||
}
|
}
|
||||||
|
|
||||||
// Calculate a least squares polynomial fit.
|
// Calculate a least squares polynomial fit.
|
||||||
if (degree > Estimator::MAX_DEGREE) {
|
uint32_t degree = DEGREE;
|
||||||
degree = Estimator::MAX_DEGREE;
|
|
||||||
}
|
|
||||||
if (degree > m - 1) {
|
if (degree > m - 1) {
|
||||||
degree = m - 1;
|
degree = m - 1;
|
||||||
}
|
}
|
||||||
|
|||||||
Reference in New Issue
Block a user