1.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
     1.2 +++ b/libovr/Src/OVR_SensorFilter.cpp	Sat Sep 14 16:14:59 2013 +0300
     1.3 @@ -0,0 +1,201 @@
     1.4 +/************************************************************************************
     1.5 +
     1.6 +PublicHeader:   OVR.h
     1.7 +Filename    :   OVR_SensorFilter.cpp
     1.8 +Content     :   Basic filtering of sensor data
     1.9 +Created     :   March 7, 2013
    1.10 +Authors     :   Steve LaValle, Anna Yershova
    1.11 +
    1.12 +Copyright   :   Copyright 2012 Oculus VR, Inc. All Rights reserved.
    1.13 +
    1.14 +Use of this software is subject to the terms of the Oculus license
    1.15 +agreement provided at the time of installation or download, or which
    1.16 +otherwise accompanies this software in either electronic or hard copy form.
    1.17 +
    1.18 +*************************************************************************************/
    1.19 +
    1.20 +#include "OVR_SensorFilter.h"
    1.21 +
    1.22 +namespace OVR {
    1.23 +
    1.24 +Vector3f SensorFilter::Total() const
    1.25 +{
    1.26 +    Vector3f total = Vector3f(0.0f, 0.0f, 0.0f);
    1.27 +    for (int i = 0; i < Size; i++)
    1.28 +        total += Elements[i];
    1.29 +    return total;
    1.30 +}
    1.31 +
    1.32 +Vector3f SensorFilter::Mean() const
    1.33 +{
    1.34 +    Vector3f total = Vector3f(0.0f, 0.0f, 0.0f);
    1.35 +    for (int i = 0; i < Size; i++)
    1.36 +        total += Elements[i];
    1.37 +    return total / (float) Size;
    1.38 +}
    1.39 +
    1.40 +Vector3f SensorFilter::Median() const
    1.41 +{
    1.42 +    int half_window = (int) Size / 2;
    1.43 +    float sortx[MaxFilterSize];
    1.44 +    float resultx = 0.0f;
    1.45 +
    1.46 +    float sorty[MaxFilterSize];
    1.47 +    float resulty = 0.0f;
    1.48 +
    1.49 +    float sortz[MaxFilterSize];
    1.50 +    float resultz = 0.0f;
    1.51 +
    1.52 +    for (int i = 0; i < Size; i++) 
    1.53 +    {
    1.54 +        sortx[i] = Elements[i].x;
    1.55 +        sorty[i] = Elements[i].y;
    1.56 +        sortz[i] = Elements[i].z;
    1.57 +    }
    1.58 +    for (int j = 0; j <= half_window; j++) 
    1.59 +    {
    1.60 +        int minx = j;
    1.61 +        int miny = j;
    1.62 +        int minz = j;
    1.63 +        for (int k = j + 1; k < Size; k++) 
    1.64 +        {
    1.65 +            if (sortx[k] < sortx[minx]) minx = k;
    1.66 +            if (sorty[k] < sorty[miny]) miny = k;
    1.67 +            if (sortz[k] < sortz[minz]) minz = k;
    1.68 +        }
    1.69 +        const float tempx = sortx[j];
    1.70 +        const float tempy = sorty[j];
    1.71 +        const float tempz = sortz[j];
    1.72 +        sortx[j] = sortx[minx];
    1.73 +        sortx[minx] = tempx;
    1.74 +
    1.75 +        sorty[j] = sorty[miny];
    1.76 +        sorty[miny] = tempy;
    1.77 +
    1.78 +        sortz[j] = sortz[minz];
    1.79 +        sortz[minz] = tempz;
    1.80 +    }
    1.81 +    resultx = sortx[half_window];
    1.82 +    resulty = sorty[half_window];
    1.83 +    resultz = sortz[half_window];
    1.84 +
    1.85 +    return Vector3f(resultx, resulty, resultz);
    1.86 +}
    1.87 +
    1.88 +//  Only the diagonal of the covariance matrix.
    1.89 +Vector3f SensorFilter::Variance() const
    1.90 +{
    1.91 +    Vector3f mean = Mean();
    1.92 +    Vector3f total = Vector3f(0.0f, 0.0f, 0.0f);
    1.93 +    for (int i = 0; i < Size; i++) 
    1.94 +    {
    1.95 +        total.x += (Elements[i].x - mean.x) * (Elements[i].x - mean.x);
    1.96 +        total.y += (Elements[i].y - mean.y) * (Elements[i].y - mean.y);
    1.97 +        total.z += (Elements[i].z - mean.z) * (Elements[i].z - mean.z);
    1.98 +    }
    1.99 +    return total / (float) Size;
   1.100 +}
   1.101 +
   1.102 +// Should be a 3x3 matrix returned, but OVR_math.h doesn't have one
   1.103 +Matrix4f SensorFilter::Covariance() const
   1.104 +{
   1.105 +    Vector3f mean = Mean();
   1.106 +    Matrix4f total = Matrix4f(0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0);
   1.107 +    for (int i = 0; i < Size; i++) 
   1.108 +    {
   1.109 +        total.M[0][0] += (Elements[i].x - mean.x) * (Elements[i].x - mean.x);
   1.110 +        total.M[1][0] += (Elements[i].y - mean.y) * (Elements[i].x - mean.x);
   1.111 +        total.M[2][0] += (Elements[i].z - mean.z) * (Elements[i].x - mean.x);
   1.112 +        total.M[1][1] += (Elements[i].y - mean.y) * (Elements[i].y - mean.y);
   1.113 +        total.M[2][1] += (Elements[i].z - mean.z) * (Elements[i].y - mean.y);
   1.114 +        total.M[2][2] += (Elements[i].z - mean.z) * (Elements[i].z - mean.z);
   1.115 +    }
   1.116 +    total.M[0][1] = total.M[1][0];
   1.117 +    total.M[0][2] = total.M[2][0];
   1.118 +    total.M[1][2] = total.M[2][1];
   1.119 +    for (int i = 0; i < 3; i++)
   1.120 +        for (int j = 0; j < 3; j++)
   1.121 +            total.M[i][j] *= 1.0f / Size;
   1.122 +    return total;
   1.123 +}
   1.124 +
   1.125 +Vector3f SensorFilter::PearsonCoefficient() const
   1.126 +{
   1.127 +    Matrix4f cov = Covariance();
   1.128 +    Vector3f pearson = Vector3f();
   1.129 +    pearson.x = cov.M[0][1]/(sqrt(cov.M[0][0])*sqrt(cov.M[1][1]));
   1.130 +    pearson.y = cov.M[1][2]/(sqrt(cov.M[1][1])*sqrt(cov.M[2][2]));
   1.131 +    pearson.z = cov.M[2][0]/(sqrt(cov.M[2][2])*sqrt(cov.M[0][0]));
   1.132 +
   1.133 +    return pearson;
   1.134 +}
   1.135 +
   1.136 +
   1.137 +Vector3f SensorFilter::SavitzkyGolaySmooth8() const
   1.138 +{
   1.139 +    OVR_ASSERT(Size >= 8);
   1.140 +    return GetPrev(0)*0.41667f +
   1.141 +            GetPrev(1)*0.33333f +
   1.142 +            GetPrev(2)*0.25f +
   1.143 +            GetPrev(3)*0.16667f +
   1.144 +            GetPrev(4)*0.08333f -
   1.145 +            GetPrev(6)*0.08333f -
   1.146 +            GetPrev(7)*0.16667f;
   1.147 +}
   1.148 +
   1.149 +
   1.150 +Vector3f SensorFilter::SavitzkyGolayDerivative4() const
   1.151 +{
   1.152 +    OVR_ASSERT(Size >= 4);
   1.153 +    return GetPrev(0)*0.3f +
   1.154 +            GetPrev(1)*0.1f -
   1.155 +            GetPrev(2)*0.1f -
   1.156 +            GetPrev(3)*0.3f;
   1.157 +}
   1.158 +
   1.159 +Vector3f SensorFilter::SavitzkyGolayDerivative5() const
   1.160 +{
   1.161 +    OVR_ASSERT(Size >= 5);
   1.162 +    return GetPrev(0)*0.2f +
   1.163 +            GetPrev(1)*0.1f -
   1.164 +            GetPrev(3)*0.1f -
   1.165 +            GetPrev(4)*0.2f;
   1.166 +}
   1.167 +
   1.168 +Vector3f SensorFilter::SavitzkyGolayDerivative12() const
   1.169 +{
   1.170 +    OVR_ASSERT(Size >= 12);
   1.171 +    return GetPrev(0)*0.03846f +
   1.172 +            GetPrev(1)*0.03147f +
   1.173 +            GetPrev(2)*0.02448f +
   1.174 +            GetPrev(3)*0.01748f +
   1.175 +            GetPrev(4)*0.01049f +
   1.176 +            GetPrev(5)*0.0035f -
   1.177 +            GetPrev(6)*0.0035f -
   1.178 +            GetPrev(7)*0.01049f -
   1.179 +            GetPrev(8)*0.01748f -
   1.180 +            GetPrev(9)*0.02448f -
   1.181 +            GetPrev(10)*0.03147f -
   1.182 +            GetPrev(11)*0.03846f;
   1.183 +}
   1.184 +
   1.185 +Vector3f SensorFilter::SavitzkyGolayDerivativeN(int n) const
   1.186 +{    
   1.187 +    OVR_ASSERT(Size >= n);
   1.188 +    int m = (n-1)/2;
   1.189 +    Vector3f result = Vector3f();
   1.190 +    for (int k = 1; k <= m; k++) 
   1.191 +    {
   1.192 +        int ind1 = m - k;
   1.193 +        int ind2 = n - m + k - 1;
   1.194 +        result += (GetPrev(ind1) - GetPrev(ind2)) * (float) k;
   1.195 +    }
   1.196 +    float coef = 3.0f/(m*(m+1.0f)*(2.0f*m+1.0f));
   1.197 +    result = result*coef;
   1.198 +    return result;
   1.199 +}
   1.200 +
   1.201 +
   1.202 +
   1.203 +
   1.204 +} //namespace OVR
   1.205 \ No newline at end of file