diff --git a/include/bx/fpumath.h b/include/bx/fpumath.h index 37d4fec..8ceeec6 100644 --- a/include/bx/fpumath.h +++ b/include/bx/fpumath.h @@ -833,6 +833,82 @@ namespace bx _result[3] = -vec3Dot(normal, _va); } + inline void calcLinearFit2D(float _result[2], const void* _points, uint32_t _stride, uint32_t _numPoints) + { + float sumX = 0.0f; + float sumY = 0.0f; + float sumXX = 0.0f; + float sumXY = 0.0f; + + const uint8_t* ptr = (const uint8_t*)_points; + for (uint32_t ii = 0; ii < _numPoints; ++ii, ptr += _stride) + { + const float* point = (const float*)ptr; + float xx = point[0]; + float yy = point[1]; + sumX += xx; + sumY += yy; + sumXX += xx*xx; + sumXY += xx*yy; + } + + // [ sum(x^2) sum(x) ] [ A ] = [ sum(x*y) ] + // [ sum(x) numPoints ] [ B ] [ sum(y) ] + + float det = (sumXX*_numPoints - sumX*sumX); + float invDet = 1.0f/det; + + _result[0] = (-sumX * sumY + _numPoints * sumXY) * invDet; + _result[1] = (sumXX * sumY - sumX * sumXY) * invDet; + } + + inline void calcLinearFit3D(float _result[3], const void* _points, uint32_t _stride, uint32_t _numPoints) + { + float sumX = 0.0f; + float sumY = 0.0f; + float sumZ = 0.0f; + float sumXX = 0.0f; + float sumXY = 0.0f; + float sumXZ = 0.0f; + float sumYY = 0.0f; + float sumYZ = 0.0f; + + const uint8_t* ptr = (const uint8_t*)_points; + for (uint32_t ii = 0; ii < _numPoints; ++ii, ptr += _stride) + { + const float* point = (const float*)ptr; + float xx = point[0]; + float yy = point[1]; + float zz = point[2]; + + sumX += xx; + sumY += yy; + sumZ += zz; + sumXX += xx*xx; + sumXY += xx*yy; + sumXZ += xx*zz; + sumYY += yy*yy; + sumYZ += yy*zz; + } + + // [ sum(x^2) sum(x*y) sum(x) ] [ A ] [ sum(x*z) ] + // [ sum(x*y) sum(y^2) sum(y) ] [ B ] = [ sum(y*z) ] + // [ sum(x) sum(y) numPoints ] [ C ] [ sum(z) ] + + float mtx[9] = + { + sumXX, sumXY, sumX, + sumXY, sumYY, sumY, + sumX, sumY, float(_numPoints), + }; + float invMtx[9]; + bx::mtx3Inverse(invMtx, mtx); + + _result[0] = invMtx[0]*sumXZ + invMtx[1]*sumYZ + invMtx[2]*sumZ; + _result[1] = invMtx[3]*sumXZ + invMtx[4]*sumYZ + invMtx[5]*sumZ; + _result[2] = invMtx[6]*sumXZ + invMtx[7]*sumYZ + invMtx[8]*sumZ; + } + inline void rgbToHsv(float _hsv[3], const float _rgb[3]) { const float rr = _rgb[0];