bool VRFrameData::update(const device::mojom::blink::VRPosePtr& pose,
VREyeParameters* leftEye,
VREyeParameters* rightEye,
float depthNear,
float depthFar) {
if (!pose)
return false;
m_timestamp = pose->timestamp;
// Build the projection matrices
projectionFromFieldOfView(m_leftProjectionMatrix, leftEye->fieldOfView(),
depthNear, depthFar);
projectionFromFieldOfView(m_rightProjectionMatrix, rightEye->fieldOfView(),
depthNear, depthFar);
// Build the view matrices
matrixfromRotationTranslation(m_leftViewMatrix, pose->orientation,
pose->position);
matrixTranslate(m_leftViewMatrix, leftEye->offset());
if (!matrixInvert(m_leftViewMatrix))
return false;
matrixfromRotationTranslation(m_rightViewMatrix, pose->orientation,
pose->position);
matrixTranslate(m_rightViewMatrix, rightEye->offset());
if (!matrixInvert(m_rightViewMatrix))
return false;
// Set the pose
m_pose->setPose(pose);
return true;
}
void getWorldOffsetPosr(const dxPosR& body_posr, const dxPosR& world_posr, dxPosR& offset_posr)
{
dMatrix3 inv_body;
matrixInvert(body_posr.R, inv_body);
dMultiply0_333(offset_posr.R, inv_body, world_posr.R);
dVector3 world_offset;
world_offset[0] = world_posr.pos[0] - body_posr.pos[0];
world_offset[1] = world_posr.pos[1] - body_posr.pos[1];
world_offset[2] = world_posr.pos[2] - body_posr.pos[2];
dMultiply0_331(offset_posr.pos, inv_body, world_offset);
}
void getBodyPosr(const dxPosR& offset_posr, const dxPosR& final_posr, dxPosR& body_posr)
{
dMatrix3 inv_offset;
matrixInvert(offset_posr.R, inv_offset);
dMultiply0_333(body_posr.R, final_posr.R, inv_offset);
dVector3 world_offset;
dMultiply0_331(world_offset, body_posr.R, offset_posr.pos);
body_posr.pos[0] = final_posr.pos[0] - world_offset[0];
body_posr.pos[1] = final_posr.pos[1] - world_offset[1];
body_posr.pos[2] = final_posr.pos[2] - world_offset[2];
}
int VizGeorefSpline2D::solve(void)
{
int r, c, v;
int p;
// No points at all
if ( _nof_points < 1 )
{
type = VIZ_GEOREF_SPLINE_ZERO_POINTS;
return(0);
}
// Only one point
if ( _nof_points == 1 )
{
type = VIZ_GEOREF_SPLINE_ONE_POINT;
return(1);
}
// Just 2 points - it is necessarily 1D case
if ( _nof_points == 2 )
{
_dx = x[1] - x[0];
_dy = y[1] - y[0];
double fact = 1.0 / ( _dx * _dx + _dy * _dy );
_dx *= fact;
_dy *= fact;
type = VIZ_GEOREF_SPLINE_TWO_POINTS;
return(2);
}
// More than 2 points - first we have to check if it is 1D or 2D case
double xmax = x[0], xmin = x[0], ymax = y[0], ymin = y[0];
double delx, dely;
double xx, yy;
double sumx = 0.0f, sumy= 0.0f, sumx2 = 0.0f, sumy2 = 0.0f, sumxy = 0.0f;
double SSxx, SSyy, SSxy;
for ( p = 0; p < _nof_points; p++ )
{
xx = x[p];
yy = y[p];
xmax = MAX( xmax, xx );
xmin = MIN( xmin, xx );
ymax = MAX( ymax, yy );
ymin = MIN( ymin, yy );
sumx += xx;
sumx2 += xx * xx;
sumy += yy;
sumy2 += yy * yy;
sumxy += xx * yy;
}
delx = xmax - xmin;
dely = ymax - ymin;
SSxx = sumx2 - sumx * sumx / _nof_points;
SSyy = sumy2 - sumy * sumy / _nof_points;
SSxy = sumxy - sumx * sumy / _nof_points;
if ( delx < 0.001 * dely || dely < 0.001 * delx ||
fabs ( SSxy * SSxy / ( SSxx * SSyy ) ) > 0.99 )
{
int p1;
type = VIZ_GEOREF_SPLINE_ONE_DIMENSIONAL;
_dx = _nof_points * sumx2 - sumx * sumx;
_dy = _nof_points * sumy2 - sumy * sumy;
double fact = 1.0 / sqrt( _dx * _dx + _dy * _dy );
_dx *= fact;
_dy *= fact;
for ( p = 0; p < _nof_points; p++ )
{
double dxp = x[p] - x[0];
double dyp = y[p] - y[0];
u[p] = _dx * dxp + _dy * dyp;
unused[p] = 1;
}
for ( p = 0; p < _nof_points; p++ )
{
int min_index = -1;
double min_u = 0;
for ( p1 = 0; p1 < _nof_points; p1++ )
{
if ( unused[p1] )
{
if ( min_index < 0 || u[p1] < min_u )
{
min_index = p1;
min_u = u[p1];
}
}
}
index[p] = min_index;
unused[min_index] = 0;
}
return(3);
}
type = VIZ_GEOREF_SPLINE_FULL;
// Make the necessary memory allocations
if ( _AA )
CPLFree(_AA);
if ( _Ainv )
CPLFree(_Ainv);
_nof_eqs = _nof_points + 3;
_AA = ( double * )CPLCalloc( _nof_eqs * _nof_eqs, sizeof( double ) );
_Ainv = ( double * )CPLCalloc( _nof_eqs * _nof_eqs, sizeof( double ) );
// Calc the values of the matrix A
for ( r = 0; r < 3; r++ )
for ( c = 0; c < 3; c++ )
A(r,c) = 0.0;
for ( c = 0; c < _nof_points; c++ )
{
A(0,c+3) = 1.0;
A(1,c+3) = x[c];
A(2,c+3) = y[c];
A(c+3,0) = 1.0;
A(c+3,1) = x[c];
A(c+3,2) = y[c];
}
for ( r = 0; r < _nof_points; r++ )
for ( c = r; c < _nof_points; c++ )
{
A(r+3,c+3) = base_func( x[r], y[r], x[c], y[c] );
if ( r != c )
A(c+3,r+3 ) = A(r+3,c+3);
}
#if VIZ_GEOREF_SPLINE_DEBUG
for ( r = 0; r < _nof_eqs; r++ )
{
for ( c = 0; c < _nof_eqs; c++ )
fprintf(stderr, "%f", A(r,c));
fprintf(stderr, "\n");
}
#endif
// Invert the matrix
int status = matrixInvert( _nof_eqs, _AA, _Ainv );
if ( !status )
{
fprintf(stderr, " There is a problem to invert the interpolation matrix\n");
return 0;
}
// calc the coefs
for ( v = 0; v < _nof_vars; v++ )
for ( r = 0; r < _nof_eqs; r++ )
{
coef[v][r] = 0.0;
for ( c = 0; c < _nof_eqs; c++ )
coef[v][r] += Ainv(r,c) * rhs[v][c];
}
return(4);
}
int VizGeorefSpline2D::solve(void)
{
int r, c;
int p;
// No points at all
if ( _nof_points < 1 )
{
type = VIZ_GEOREF_SPLINE_ZERO_POINTS;
return(0);
}
// Only one point
if ( _nof_points == 1 )
{
type = VIZ_GEOREF_SPLINE_ONE_POINT;
return(1);
}
// Just 2 points - it is necessarily 1D case
if ( _nof_points == 2 )
{
_dx = x[1] - x[0];
_dy = y[1] - y[0];
double fact = 1.0 / ( _dx * _dx + _dy * _dy );
_dx *= fact;
_dy *= fact;
type = VIZ_GEOREF_SPLINE_TWO_POINTS;
return(2);
}
// More than 2 points - first we have to check if it is 1D or 2D case
double xmax = x[0], xmin = x[0], ymax = y[0], ymin = y[0];
double delx, dely;
double xx, yy;
double sumx = 0.0f, sumy= 0.0f, sumx2 = 0.0f, sumy2 = 0.0f, sumxy = 0.0f;
double SSxx, SSyy, SSxy;
for ( p = 0; p < _nof_points; p++ )
{
xx = x[p];
yy = y[p];
xmax = MAX( xmax, xx );
xmin = MIN( xmin, xx );
ymax = MAX( ymax, yy );
ymin = MIN( ymin, yy );
sumx += xx;
sumx2 += xx * xx;
sumy += yy;
sumy2 += yy * yy;
sumxy += xx * yy;
}
delx = xmax - xmin;
dely = ymax - ymin;
SSxx = sumx2 - sumx * sumx / _nof_points;
SSyy = sumy2 - sumy * sumy / _nof_points;
SSxy = sumxy - sumx * sumy / _nof_points;
if ( delx < 0.001 * dely || dely < 0.001 * delx ||
fabs ( SSxy * SSxy / ( SSxx * SSyy ) ) > 0.99 )
{
int p1;
type = VIZ_GEOREF_SPLINE_ONE_DIMENSIONAL;
_dx = _nof_points * sumx2 - sumx * sumx;
_dy = _nof_points * sumy2 - sumy * sumy;
double fact = 1.0 / sqrt( _dx * _dx + _dy * _dy );
_dx *= fact;
_dy *= fact;
for ( p = 0; p < _nof_points; p++ )
{
double dxp = x[p] - x[0];
double dyp = y[p] - y[0];
u[p] = _dx * dxp + _dy * dyp;
unused[p] = 1;
}
for ( p = 0; p < _nof_points; p++ )
{
int min_index = -1;
double min_u = 0;
for ( p1 = 0; p1 < _nof_points; p1++ )
{
if ( unused[p1] )
{
if ( min_index < 0 || u[p1] < min_u )
{
min_index = p1;
min_u = u[p1];
}
}
}
index[p] = min_index;
unused[min_index] = 0;
}
return(3);
}
type = VIZ_GEOREF_SPLINE_FULL;
// Make the necessary memory allocations
_nof_eqs = _nof_points + 3;
if( _nof_eqs > INT_MAX / _nof_eqs )
{
CPLError(CE_Failure, CPLE_AppDefined, "Too many coefficients. Computation aborted.");
return 0;
}
double* _AA = ( double * )VSICalloc( _nof_eqs * _nof_eqs, sizeof( double ) );
double* _Ainv = ( double * )VSICalloc( _nof_eqs * _nof_eqs, sizeof( double ) );
if( _AA == NULL || _Ainv == NULL )
{
CPLError(CE_Failure, CPLE_AppDefined, "Out-of-memory while allocating temporary arrays. Computation aborted.");
VSIFree(_AA);
VSIFree(_Ainv);
return 0;
}
// Calc the values of the matrix A
for ( r = 0; r < 3; r++ )
for ( c = 0; c < 3; c++ )
A(r,c) = 0.0;
for ( c = 0; c < _nof_points; c++ )
{
A(0,c+3) = 1.0;
A(1,c+3) = x[c];
A(2,c+3) = y[c];
A(c+3,0) = 1.0;
A(c+3,1) = x[c];
A(c+3,2) = y[c];
}
for ( r = 0; r < _nof_points; r++ )
for ( c = r; c < _nof_points; c++ )
{
A(r+3,c+3) = VizGeorefSpline2DBase_func( x[r], y[r], x[c], y[c] );
if ( r != c )
A(c+3,r+3 ) = A(r+3,c+3);
}
#if VIZ_GEOREF_SPLINE_DEBUG
for ( r = 0; r < _nof_eqs; r++ )
{
for ( c = 0; c < _nof_eqs; c++ )
fprintf(stderr, "%f", A(r,c));
fprintf(stderr, "\n");
}
#endif
int ret = 4;
#ifdef HAVE_ARMADILLO
try
{
arma::mat matA(_AA,_nof_eqs,_nof_eqs,false);
arma::mat matRHS(_nof_eqs, _nof_vars);
int row, col;
for(row = 0; row < _nof_eqs; row++)
for(col = 0; col < _nof_vars; col++)
matRHS.at(row, col) = rhs[col][row];
arma::mat matCoefs(_nof_vars, _nof_eqs);
if( !arma::solve(matCoefs, matA, matRHS) )
{
CPLError(CE_Failure, CPLE_AppDefined, "There is a problem to invert the interpolation matrix.");
ret = 0;
}
else
{
for(row = 0; row < _nof_eqs; row++)
for(col = 0; col < _nof_vars; col++)
coef[col][row] = matCoefs.at(row, col);
}
}
catch(...)
{
CPLError(CE_Failure, CPLE_AppDefined, "There is a problem to invert the interpolation matrix.");
ret = 0;
}
#else
// Invert the matrix
int status = matrixInvert( _nof_eqs, _AA, _Ainv );
if ( !status )
{
CPLError(CE_Failure, CPLE_AppDefined, "There is a problem to invert the interpolation matrix.");
ret = 0;
}
else
{
// calc the coefs
for ( int v = 0; v < _nof_vars; v++ )
for ( r = 0; r < _nof_eqs; r++ )
{
coef[v][r] = 0.0;
for ( c = 0; c < _nof_eqs; c++ )
coef[v][r] += Ainv(r,c) * rhs[v][c];
}
}
#endif
VSIFree(_AA);
VSIFree(_Ainv);
return(ret);
}
// LLSQ Approx. Trilateration
void locateLLSQ(int numantennas, double* antennas, double* distances, double* x)
{
// function saves triangulated position in vector x
// input *antennas holds
// x1, y1, z1,
// x2, y2, z2,
// x3, y3, z3,
// x4, y4, z4;
// numantennas holds integer number of antennas (min 5)
// double *distances holds pointer to array holding distances from each antenna
// Theory used:
// http://inside.mines.edu/~whereman/talks/TurgutOzal-11-Trilateration.pdf
// define some locals
int m = (numantennas - 1); // Number of rows in A
int n = 3; // Number of columns in A (always three)
if (m >= 4)n = 4; // Number of columns in A: three coordinates plus K
double A[m * n];
double b[m * 1];
double r;
double Atranspose[n * m];
double AtAinA[n * m];
double AtA[n * n];
//Calculating b vector
for (int i = 0; i < m; i++)
{
r = dist(antennas[0], antennas[1], antennas[2], antennas[(i + 1) * 3], antennas[(i + 1) * 3 + 1], antennas[(i + 1) * 3 + 2]);
b[i] = 0.5*(distances[0] * distances[0] - distances[i + 1] * distances[i + 1] + r * r); // If given exact distances
if (n == 4)b[i] = 0.5 * r*r; // For the case when we calculate K, overwrite b
}
#ifdef VERBOSE
matrixPrint(b, m, 1, "b");
#endif // DEBUG
//Calculating A matrix
for (int i = 0; i < m; i++)
{
A[i * n] = antennas[(i + 1) * 3] - antennas[0]; //xi-x1
A[i * n + 1] = antennas[(i + 1) * 3 + 1] - antennas[1]; //yi-y1
A[i * n + 2] = antennas[(i + 1) * 3 + 2] - antennas[2]; //zi-z1
if (n == 4) A[i * n + 3] = -0.5 * (distances[0] * distances[0] - distances[i + 1] * distances[i + 1]); // for K
}
#ifdef VERBOSE
matrixPrint(A, m, n, "A");
#endif // DEBUG
matrixTranspose(A, m, n, Atranspose);
matrixMult(Atranspose, A, n, m, n, AtA);
if (matrixInvert(AtA, n) == 1) //well behaved
{
matrixMult(AtA, Atranspose, n, n, m, AtAinA);
matrixMult(AtAinA, b, n, m, 1, x);
}
else // Ill-behaved A
{
double Q[m * m], Qtranspose[m * m], Qtransposeb[m * 1];
double R[m * m];
QR(A, m, n, Q, R);
matrixTranspose(Q, m, m, Qtranspose);
matrixMult(Qtranspose, b, m, m, 1, Qtransposeb);
matrixInvert(R, m);
matrixMult(R, Qtransposeb, m, m, 1, x);
}
// Adding back the reference point
x[0] = x[0] + antennas[0];
x[1] = x[1] + antennas[1];
x[2] = x[2] + antennas[2];
if (n == 4)x[3] = 1 / sqrt(x[3]); // Calculate K
}
