780 lines
20 KiB
780 lines
20 KiB
/*
|
|
|
|
Numerical functions for computing minimizers of a least-squares system
|
|
of equations.
|
|
|
|
Copyright (C) 2011 Scott Schaefer
|
|
|
|
This library is free software; you can redistribute it and/or
|
|
modify it under the terms of the GNU Lesser General Public License
|
|
(LGPL) as published by the Free Software Foundation; either
|
|
version 2.1 of the License, or (at your option) any later version.
|
|
|
|
This library is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
|
Lesser General Public License for more details.
|
|
|
|
You should have received a copy of the GNU Lesser General Public
|
|
License along with this library; if not, write to the Free Software
|
|
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
|
*/
|
|
|
|
|
|
#include "eigen.h"
|
|
|
|
#include <math.h>
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <assert.h>
|
|
#define ROTATE(a,i,j,k,l) g=a[i][j];h=a[k][l];a[i][j]=g-s*(h+g*tau);a[k][l]=h+s*(g-h*tau);
|
|
|
|
int method = 3;
|
|
|
|
// for reducing two upper triangular systems of equations into 1
|
|
void qr ( float *mat1, float *mat2, float *rvalue )
|
|
{
|
|
int i, j;
|
|
float temp1 [ 8 ] [ 4 ];
|
|
|
|
for ( i = 0; i < 4; i++ )
|
|
{
|
|
for ( j = 0; j < i; j++ )
|
|
{
|
|
temp1 [ i ] [ j ] = 0;
|
|
temp1 [ i + 4 ] [ j ] = 0;
|
|
}
|
|
for ( j = i; j < 4; j++ )
|
|
{
|
|
temp1 [ i ] [ j ] = mat1 [ ( 7 * i - i * i ) / 2 + j ];
|
|
temp1 [ i + 4 ] [ j ] = mat2 [ ( 7 * i - i * i ) / 2 + j ];
|
|
}
|
|
}
|
|
|
|
qr ( temp1, 8, rvalue );
|
|
}
|
|
|
|
// WARNING: destroys eqs in the process
|
|
void qr ( float eqs[][4], int num, float *rvalue )
|
|
{
|
|
int i, j, k;
|
|
|
|
qr ( eqs, num, 0.000001f );
|
|
for ( i = 0; i < 10; i++ )
|
|
{
|
|
rvalue [ i ] = 0;
|
|
}
|
|
|
|
k = 0;
|
|
for ( i = 0; i < num && i < 4; i++ )
|
|
{
|
|
for ( j = i; j < 4; j++ )
|
|
{
|
|
rvalue [ k++ ] = eqs [ i ] [ j ];
|
|
}
|
|
}
|
|
}
|
|
|
|
void qr ( float eqs[][4], int num, float tol )
|
|
{
|
|
int i, j, k;
|
|
float a, b, mag, temp;
|
|
|
|
for ( i = 0; i < 4 && i < num; i++ )
|
|
{
|
|
for ( j = i + 1; j < num; j++ )
|
|
{
|
|
a = eqs [ i ] [ i ];
|
|
b = eqs [ j ] [ i ];
|
|
|
|
if ( fabs ( a ) > 0.000001f || fabs ( b ) > 0.000001f )
|
|
{
|
|
mag = (float)sqrt ( a * a + b * b );
|
|
a /= mag;
|
|
b /= mag;
|
|
|
|
for ( k = 0; k < 4; k++ )
|
|
{
|
|
temp = a * eqs [ i ] [ k ] + b * eqs [ j ] [ k ];
|
|
eqs [ j ] [ k ] = b * eqs [ i ] [ k ] - a * eqs [ j ] [ k ];
|
|
eqs [ i ] [ k ] = temp;
|
|
}
|
|
}
|
|
}
|
|
for ( j = i - 1; j >= 0; j-- )
|
|
{
|
|
if ( eqs [ j ] [ j ] < 0.000001f && eqs [ j ] [ j ] > -0.000001f )
|
|
{
|
|
a = eqs [ i ] [ i ];
|
|
b = eqs [ j ] [ i ];
|
|
|
|
if ( fabs ( a ) > 0.000001f || fabs ( b ) > 0.000001f )
|
|
{
|
|
mag = (float)sqrt ( a * a + b * b );
|
|
a /= mag;
|
|
b /= mag;
|
|
|
|
for ( k = 0; k < 4; k++ )
|
|
{
|
|
temp = a * eqs [ i ] [ k ] + b * eqs [ j ] [ k ];
|
|
eqs [ j ] [ k ] = b * eqs [ i ] [ k ] - a * eqs [ j ] [ k ];
|
|
eqs [ i ] [ k ] = temp;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
void jacobi ( float u[][3], float d[], float v[][3] )
|
|
{
|
|
int j, iq, ip, i;
|
|
float tresh, theta, tau, t, sm, s, h, g, c, b [ 3 ], z [ 3 ];
|
|
float a [ 3 ] [ 3 ];
|
|
|
|
a [ 0 ] [ 0 ] = u [ 0 ] [ 0 ];
|
|
a [ 0 ] [ 1 ] = u [ 0 ] [ 1 ];
|
|
a [ 0 ] [ 2 ] = u [ 0 ] [ 2 ];
|
|
a [ 1 ] [ 0 ] = u [ 1 ] [ 0 ];
|
|
a [ 1 ] [ 1 ] = u [ 1 ] [ 1 ];
|
|
a [ 1 ] [ 2 ] = u [ 1 ] [ 2 ];
|
|
a [ 2 ] [ 0 ] = u [ 2 ] [ 0 ];
|
|
a [ 2 ] [ 1 ] = u [ 2 ] [ 1 ];
|
|
a [ 2 ] [ 2 ] = u [ 2 ] [ 2 ];
|
|
|
|
for ( ip = 0; ip < 3; ip++ )
|
|
{
|
|
for ( iq = 0; iq < 3; iq++ )
|
|
{
|
|
v [ ip ] [ iq ] = 0.0f;
|
|
}
|
|
v [ ip ] [ ip ] = 1.0f;
|
|
}
|
|
|
|
for ( ip = 0; ip < 3; ip++ )
|
|
{
|
|
b [ ip ] = a [ ip ] [ ip ];
|
|
d [ ip ] = b [ ip ];
|
|
z [ ip ] = 0.0f;
|
|
}
|
|
|
|
for ( i = 1; i <= 50; i++ )
|
|
{
|
|
sm = 0.0f;
|
|
for ( ip = 0; ip < 2; ip++ )
|
|
{
|
|
for ( iq = ip + 1; iq < 3; iq++ )
|
|
{
|
|
sm += (float)fabs ( a [ ip ] [ iq ] );
|
|
}
|
|
}
|
|
|
|
if ( sm == 0.0f )
|
|
{
|
|
// sort the stupid things and transpose
|
|
a [ 0 ] [ 0 ] = v [ 0 ] [ 0 ];
|
|
a [ 0 ] [ 1 ] = v [ 1 ] [ 0 ];
|
|
a [ 0 ] [ 2 ] = v [ 2 ] [ 0 ];
|
|
a [ 1 ] [ 0 ] = v [ 0 ] [ 1 ];
|
|
a [ 1 ] [ 1 ] = v [ 1 ] [ 1 ];
|
|
a [ 1 ] [ 2 ] = v [ 2 ] [ 1 ];
|
|
a [ 2 ] [ 0 ] = v [ 0 ] [ 2 ];
|
|
a [ 2 ] [ 1 ] = v [ 1 ] [ 2 ];
|
|
a [ 2 ] [ 2 ] = v [ 2 ] [ 2 ];
|
|
|
|
if ( fabs ( d [ 0 ] ) < fabs ( d [ 1 ] ) )
|
|
{
|
|
sm = d [ 0 ];
|
|
d [ 0 ] = d [ 1 ];
|
|
d [ 1 ] = sm;
|
|
|
|
sm = a [ 0 ] [ 0 ];
|
|
a [ 0 ] [ 0 ] = a [ 1 ] [ 0 ];
|
|
a [ 1 ] [ 0 ] = sm;
|
|
sm = a [ 0 ] [ 1 ];
|
|
a [ 0 ] [ 1 ] = a [ 1 ] [ 1 ];
|
|
a [ 1 ] [ 1 ] = sm;
|
|
sm = a [ 0 ] [ 2 ];
|
|
a [ 0 ] [ 2 ] = a [ 1 ] [ 2 ];
|
|
a [ 1 ] [ 2 ] = sm;
|
|
}
|
|
if ( fabs ( d [ 1 ] ) < fabs ( d [ 2 ] ) )
|
|
{
|
|
sm = d [ 1 ];
|
|
d [ 1 ] = d [ 2 ];
|
|
d [ 2 ] = sm;
|
|
|
|
sm = a [ 1 ] [ 0 ];
|
|
a [ 1] [ 0 ] = a [ 2 ] [ 0 ];
|
|
a [ 2 ] [ 0 ] = sm;
|
|
sm = a [ 1 ] [ 1 ];
|
|
a [ 1 ] [ 1 ] = a [ 2 ] [ 1 ];
|
|
a [ 2 ] [ 1 ] = sm;
|
|
sm = a [ 1 ] [ 2 ];
|
|
a [ 1 ] [ 2 ] = a [ 2 ] [ 2 ];
|
|
a [ 2 ] [ 2 ] = sm;
|
|
}
|
|
if ( fabs ( d [ 0 ] ) < fabs ( d [ 1 ] ) )
|
|
{
|
|
sm = d [ 0 ];
|
|
d [ 0 ] = d [ 1 ];
|
|
d [ 1 ] = sm;
|
|
|
|
sm = a [ 0 ] [ 0 ];
|
|
a [ 0 ] [ 0 ] = a [ 1 ] [ 0 ];
|
|
a [ 1 ] [ 0 ] = sm;
|
|
sm = a [ 0 ] [ 1 ];
|
|
a [ 0 ] [ 1 ] = a [ 1 ] [ 1 ];
|
|
a [ 1 ] [ 1 ] = sm;
|
|
sm = a [ 0 ] [ 2 ];
|
|
a [ 0 ] [ 2 ] = a [ 1 ] [ 2 ];
|
|
a [ 1 ] [ 2 ] = sm;
|
|
}
|
|
|
|
v [ 0 ] [ 0 ] = a [ 0 ] [ 0 ];
|
|
v [ 0 ] [ 1 ] = a [ 0 ] [ 1 ];
|
|
v [ 0 ] [ 2 ] = a [ 0 ] [ 2 ];
|
|
v [ 1 ] [ 0 ] = a [ 1 ] [ 0 ];
|
|
v [ 1 ] [ 1 ] = a [ 1 ] [ 1 ];
|
|
v [ 1 ] [ 2 ] = a [ 1 ] [ 2 ];
|
|
v [ 2 ] [ 0 ] = a [ 2 ] [ 0 ];
|
|
v [ 2 ] [ 1 ] = a [ 2 ] [ 1 ];
|
|
v [ 2 ] [ 2 ] = a [ 2 ] [ 2 ];
|
|
|
|
return;
|
|
}
|
|
|
|
if ( i < 4 )
|
|
{
|
|
tresh = 0.2f * sm / 9;
|
|
}
|
|
else
|
|
{
|
|
tresh = 0.0f;
|
|
}
|
|
|
|
for ( ip = 0; ip < 2; ip++ )
|
|
{
|
|
for ( iq = ip + 1; iq < 3; iq++ )
|
|
{
|
|
g = 100.0f * (float)fabs ( a [ ip ] [ iq ] );
|
|
if ( i > 4 && (float)( fabs ( d [ ip ] ) + g ) == (float)fabs ( d [ ip ] )
|
|
&& (float)( fabs ( d [ iq ] ) + g ) == (float)fabs ( d [ iq ] ) )
|
|
{
|
|
a [ ip ] [ iq ] = 0.0f;
|
|
}
|
|
else
|
|
{
|
|
if ( fabs ( a [ ip ] [ iq ] ) > tresh )
|
|
{
|
|
h = d [ iq ] - d [ ip ];
|
|
if ( (float)( fabs ( h ) + g ) == (float)fabs ( h ) )
|
|
{
|
|
t = ( a [ ip ] [ iq ] ) / h;
|
|
}
|
|
else
|
|
{
|
|
theta = 0.5f * h / ( a [ ip ] [ iq ] );
|
|
t = 1.0f / ( (float)fabs ( theta ) + (float)sqrt ( 1.0f + theta * theta ) );
|
|
if ( theta < 0.0f )
|
|
{
|
|
t = -1.0f * t;
|
|
}
|
|
}
|
|
|
|
c = 1.0f / (float)sqrt ( 1 + t * t );
|
|
s = t * c;
|
|
tau = s / ( 1.0f + c );
|
|
h = t * a [ ip ] [ iq ];
|
|
z [ ip ] -= h;
|
|
z [ iq ] += h;
|
|
d [ ip ] -= h;
|
|
d [ iq ] += h;
|
|
a [ ip ] [ iq ] = 0.0f;
|
|
for ( j = 0; j <= ip - 1; j++ )
|
|
{
|
|
ROTATE ( a, j, ip, j, iq )
|
|
}
|
|
for ( j = ip + 1; j <= iq - 1; j++ )
|
|
{
|
|
ROTATE ( a, ip, j, j, iq )
|
|
}
|
|
for ( j = iq + 1; j < 3; j++ )
|
|
{
|
|
ROTATE ( a, ip, j, iq, j )
|
|
}
|
|
for ( j = 0; j < 3; j++ )
|
|
{
|
|
ROTATE ( v, j, ip, j, iq )
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
for ( ip = 0; ip < 3; ip++ )
|
|
{
|
|
b [ ip ] += z [ ip ];
|
|
d [ ip ] = b [ ip ];
|
|
z [ ip ] = 0.0f;
|
|
}
|
|
}
|
|
printf ( "too many iterations in jacobi\n" );
|
|
exit ( 1 );
|
|
}
|
|
|
|
int estimateRank ( float *a )
|
|
{
|
|
float w [ 3 ];
|
|
float u [ 3 ] [ 3 ];
|
|
float mat [ 3 ] [ 3 ];
|
|
int i;
|
|
|
|
mat [ 0 ] [ 0 ] = a [ 0 ];
|
|
mat [ 0 ] [ 1 ] = a [ 1 ];
|
|
mat [ 0 ] [ 2 ] = a [ 2 ];
|
|
mat [ 1 ] [ 1 ] = a [ 3 ];
|
|
mat [ 1 ] [ 2 ] = a [ 4 ];
|
|
mat [ 2 ] [ 2 ] = a [ 5 ];
|
|
mat [ 1 ] [ 0 ] = a [ 1 ];
|
|
mat [ 2 ] [ 0 ] = a [ 2 ];
|
|
mat [ 2 ] [ 1 ] = a [ 4 ];
|
|
|
|
jacobi ( mat, w, u );
|
|
|
|
if ( w [ 0 ] == 0.0f )
|
|
{
|
|
return 0;
|
|
}
|
|
else
|
|
{
|
|
for ( i = 1; i < 3; i++ )
|
|
{
|
|
if ( w [ i ] < 0.1f )
|
|
{
|
|
return i;
|
|
}
|
|
}
|
|
|
|
return 3;
|
|
}
|
|
|
|
}
|
|
|
|
void matInverse ( float mat[][3], float midpoint[], float rvalue[][3], float w[], float u[][3] )
|
|
{
|
|
// there is an implicit assumption that mat is symmetric and real
|
|
// U and V in the SVD will then be the same matrix whose rows are the eigenvectors of mat
|
|
// W will just be the eigenvalues of mat
|
|
// float w [ 3 ];
|
|
// float u [ 3 ] [ 3 ];
|
|
int i;
|
|
|
|
jacobi ( mat, w, u );
|
|
|
|
if ( w [ 0 ] == 0.0f )
|
|
{
|
|
// printf ( "error: largest eigenvalue is 0!\n" );
|
|
}
|
|
else
|
|
{
|
|
for ( i = 1; i < 3; i++ )
|
|
{
|
|
if ( w [ i ] < 0.001f ) // / w [ 0 ] < TOLERANCE )
|
|
{
|
|
w [ i ] = 0;
|
|
}
|
|
else
|
|
{
|
|
w [ i ] = 1.0f / w [ i ];
|
|
}
|
|
}
|
|
w [ 0 ] = 1.0f / w [ 0 ];
|
|
}
|
|
|
|
rvalue [ 0 ] [ 0 ] = w [ 0 ] * u [ 0 ] [ 0 ] * u [ 0 ] [ 0 ] +
|
|
w [ 1 ] * u [ 1 ] [ 0 ] * u [ 1 ] [ 0 ] +
|
|
w [ 2 ] * u [ 2 ] [ 0 ] * u [ 2 ] [ 0 ];
|
|
rvalue [ 0 ] [ 1 ] = w [ 0 ] * u [ 0 ] [ 0 ] * u [ 0 ] [ 1 ] +
|
|
w [ 1 ] * u [ 1 ] [ 0 ] * u [ 1 ] [ 1 ] +
|
|
w [ 2 ] * u [ 2 ] [ 0 ] * u [ 2 ] [ 1 ];
|
|
rvalue [ 0 ] [ 2 ] = w [ 0 ] * u [ 0 ] [ 0 ] * u [ 0 ] [ 2 ] +
|
|
w [ 1 ] * u [ 1 ] [ 0 ] * u [ 1 ] [ 2 ] +
|
|
w [ 2 ] * u [ 2 ] [ 0 ] * u [ 2 ] [ 2 ];
|
|
rvalue [ 1 ] [ 0 ] = w [ 0 ] * u [ 0 ] [ 1 ] * u [ 0 ] [ 0 ] +
|
|
w [ 1 ] * u [ 1 ] [ 1 ] * u [ 1 ] [ 0 ] +
|
|
w [ 2 ] * u [ 2 ] [ 1 ] * u [ 2 ] [ 0 ];
|
|
rvalue [ 1 ] [ 1 ] = w [ 0 ] * u [ 0 ] [ 1 ] * u [ 0 ] [ 1 ] +
|
|
w [ 1 ] * u [ 1 ] [ 1 ] * u [ 1 ] [ 1 ] +
|
|
w [ 2 ] * u [ 2 ] [ 1 ] * u [ 2 ] [ 1 ];
|
|
rvalue [ 1 ] [ 2 ] = w [ 0 ] * u [ 0 ] [ 1 ] * u [ 0 ] [ 2 ] +
|
|
w [ 1 ] * u [ 1 ] [ 1 ] * u [ 1 ] [ 2 ] +
|
|
w [ 2 ] * u [ 2 ] [ 1 ] * u [ 2 ] [ 2 ];
|
|
rvalue [ 2 ] [ 0 ] = w [ 0 ] * u [ 0 ] [ 2 ] * u [ 0 ] [ 0 ] +
|
|
w [ 1 ] * u [ 1 ] [ 2 ] * u [ 1 ] [ 0 ] +
|
|
w [ 2 ] * u [ 2 ] [ 2 ] * u [ 2 ] [ 0 ];
|
|
rvalue [ 2 ] [ 1 ] = w [ 0 ] * u [ 0 ] [ 2 ] * u [ 0 ] [ 1 ] +
|
|
w [ 1 ] * u [ 1 ] [ 2 ] * u [ 1 ] [ 1 ] +
|
|
w [ 2 ] * u [ 2 ] [ 2 ] * u [ 2 ] [ 1 ];
|
|
rvalue [ 2 ] [ 2 ] = w [ 0 ] * u [ 0 ] [ 2 ] * u [ 0 ] [ 2 ] +
|
|
w [ 1 ] * u [ 1 ] [ 2 ] * u [ 1 ] [ 2 ] +
|
|
w [ 2 ] * u [ 2 ] [ 2 ] * u [ 2 ] [ 2 ];
|
|
}
|
|
|
|
float calcError ( float a[][3], float b[], float btb, float point[] )
|
|
{
|
|
float rvalue = btb;
|
|
|
|
rvalue += -2.0f * ( point [ 0 ] * b [ 0 ] + point [ 1 ] * b [ 1 ] + point [ 2 ] * b [ 2 ] );
|
|
rvalue += point [ 0 ] * ( a [ 0 ] [ 0 ] * point [ 0 ] + a [ 0 ] [ 1 ] * point [ 1 ] + a [ 0 ] [ 2 ] * point [ 2 ] );
|
|
rvalue += point [ 1 ] * ( a [ 1 ] [ 0 ] * point [ 0 ] + a [ 1 ] [ 1 ] * point [ 1 ] + a [ 1 ] [ 2 ] * point [ 2 ] );
|
|
rvalue += point [ 2 ] * ( a [ 2 ] [ 0 ] * point [ 0 ] + a [ 2 ] [ 1 ] * point [ 1 ] + a [ 2 ] [ 2 ] * point [ 2 ] );
|
|
|
|
return rvalue;
|
|
}
|
|
|
|
float *calcNormal ( float halfA[], float norm[], float expectedNorm[] )
|
|
{
|
|
/*
|
|
float a [ 3 ] [ 3 ];
|
|
float w [ 3 ];
|
|
float u [ 3 ] [ 3 ];
|
|
|
|
a [ 0 ] [ 0 ] = halfA [ 0 ];
|
|
a [ 0 ] [ 1 ] = halfA [ 1 ];
|
|
a [ 0 ] [ 2 ] = halfA [ 2 ];
|
|
a [ 1 ] [ 1 ] = halfA [ 3 ];
|
|
a [ 1 ] [ 2 ] = halfA [ 4 ];
|
|
a [ 1 ] [ 0 ] = halfA [ 1 ];
|
|
a [ 2 ] [ 0 ] = halfA [ 2 ];
|
|
a [ 2 ] [ 1 ] = halfA [ 4 ];
|
|
a [ 2 ] [ 2 ] = halfA [ 5 ];
|
|
|
|
jacobi ( a, w, u );
|
|
|
|
if ( u [ 1 ] != 0 )
|
|
{
|
|
if ( w [ 1 ] / w [ 0 ] > 0.2f )
|
|
{
|
|
// two dominant eigen values, just return the expectedNorm
|
|
norm [ 0 ] = expectedNorm [ 0 ];
|
|
norm [ 1 ] = expectedNorm [ 1 ];
|
|
norm [ 2 ] = expectedNorm [ 2 ];
|
|
return;
|
|
}
|
|
}
|
|
|
|
norm [ 0 ] = u [ 0 ] [ 0 ];
|
|
norm [ 1 ] = u [ 0 ] [ 1 ];
|
|
norm [ 2 ] = u [ 0 ] [ 2 ];
|
|
*/
|
|
float dot = norm [ 0 ] * expectedNorm [ 0 ] + norm [ 1 ] * expectedNorm [ 1 ] +
|
|
norm [ 2 ] * expectedNorm [ 2 ];
|
|
|
|
if ( dot < 0 )
|
|
{
|
|
norm [ 0 ] *= -1.0f;
|
|
norm [ 1 ] *= -1.0f;
|
|
norm [ 2 ] *= -1.0f;
|
|
|
|
dot *= -1.0f;
|
|
}
|
|
|
|
if ( dot < 0.707f )
|
|
{
|
|
return expectedNorm;
|
|
}
|
|
else
|
|
{
|
|
return norm;
|
|
}
|
|
}
|
|
|
|
void descent ( float A[][3], float B[], float guess[], BoundingBoxf *box )
|
|
{
|
|
int i;
|
|
float r [ 3 ];
|
|
float delta, delta0;
|
|
int n = 10;
|
|
float alpha, div;
|
|
float newPoint [ 3 ];
|
|
float c;
|
|
float store [ 3 ];
|
|
|
|
store [ 0 ] = guess [ 0 ];
|
|
store [ 1 ] = guess [ 1 ];
|
|
store [ 2 ] = guess [ 2 ];
|
|
|
|
if ( method == 2 || method == 0 )
|
|
{
|
|
|
|
i = 0;
|
|
r [ 0 ] = B [ 0 ] - ( A [ 0 ] [ 0 ] * guess [ 0 ] + A [ 0 ] [ 1 ] * guess [ 1 ] + A [ 0 ] [ 2 ] * guess [ 2 ] );
|
|
r [ 1 ] = B [ 1 ] - ( A [ 1 ] [ 0 ] * guess [ 0 ] + A [ 1 ] [ 1 ] * guess [ 1 ] + A [ 1 ] [ 2 ] * guess [ 2 ] );
|
|
r [ 2 ] = B [ 2 ] - ( A [ 2 ] [ 0 ] * guess [ 0 ] + A [ 2 ] [ 1 ] * guess [ 1 ] + A [ 2 ] [ 2 ] * guess [ 2 ] );
|
|
|
|
delta = r [ 0 ] * r [ 0 ] + r [ 1 ] * r [ 1 ] + r [ 2 ] * r [ 2 ];
|
|
delta0 = delta * TOLERANCE * TOLERANCE;
|
|
|
|
while ( i < n && delta > delta0 )
|
|
{
|
|
div = r [ 0 ] * ( A [ 0 ] [ 0 ] * r [ 0 ] + A [ 0 ] [ 1 ] * r [ 1 ] + A [ 0 ] [ 2 ] * r [ 2 ] );
|
|
div += r [ 1 ] * ( A [ 1 ] [ 0 ] * r [ 0 ] + A [ 1 ] [ 1 ] * r [ 1 ] + A [ 1 ] [ 2 ] * r [ 2 ] );
|
|
div += r [ 2 ] * ( A [ 2 ] [ 0 ] * r [ 0 ] + A [ 2 ] [ 1 ] * r [ 1 ] + A [ 2 ] [ 2 ] * r [ 2 ] );
|
|
|
|
if ( fabs ( div ) < 0.0000001f )
|
|
{
|
|
break;
|
|
}
|
|
|
|
alpha = delta / div;
|
|
|
|
newPoint [ 0 ] = guess [ 0 ] + alpha * r [ 0 ];
|
|
newPoint [ 1 ] = guess [ 1 ] + alpha * r [ 1 ];
|
|
newPoint [ 2 ] = guess [ 2 ] + alpha * r [ 2 ];
|
|
|
|
guess [ 0 ] = newPoint [ 0 ];
|
|
guess [ 1 ] = newPoint [ 1 ];
|
|
guess [ 2 ] = newPoint [ 2 ];
|
|
|
|
r [ 0 ] = B [ 0 ] - ( A [ 0 ] [ 0 ] * guess [ 0 ] + A [ 0 ] [ 1 ] * guess [ 1 ] + A [ 0 ] [ 2 ] * guess [ 2 ] );
|
|
r [ 1 ] = B [ 1 ] - ( A [ 1 ] [ 0 ] * guess [ 0 ] + A [ 1 ] [ 1 ] * guess [ 1 ] + A [ 1 ] [ 2 ] * guess [ 2 ] );
|
|
r [ 2 ] = B [ 2 ] - ( A [ 2 ] [ 0 ] * guess [ 0 ] + A [ 2 ] [ 1 ] * guess [ 1 ] + A [ 2 ] [ 2 ] * guess [ 2 ] );
|
|
|
|
delta = r [ 0 ] * r [ 0 ] + r [ 1 ] * r [ 1 ] + r [ 2 ] * r [ 2 ];
|
|
|
|
i++;
|
|
}
|
|
|
|
if ( guess [ 0 ] >= box->begin.x && guess [ 0 ] <= box->end.x &&
|
|
guess [ 1 ] >= box->begin.y && guess [ 1 ] <= box->end.y &&
|
|
guess [ 2 ] >= box->begin.z && guess [ 2 ] <= box->end.z )
|
|
{
|
|
return;
|
|
}
|
|
}
|
|
|
|
if ( method == 0 || method == 1 )
|
|
{
|
|
c = A [ 0 ] [ 0 ] + A [ 1 ] [ 1 ] + A [ 2 ] [ 2 ];
|
|
if ( c == 0 )
|
|
{
|
|
return;
|
|
}
|
|
c = ( 0.75f / c );
|
|
|
|
guess [ 0 ] = store [ 0 ];
|
|
guess [ 1 ] = store [ 1 ];
|
|
guess [ 2 ] = store [ 2 ];
|
|
|
|
r [ 0 ] = B [ 0 ] - ( A [ 0 ] [ 0 ] * guess [ 0 ] + A [ 0 ] [ 1 ] * guess [ 1 ] + A [ 0 ] [ 2 ] * guess [ 2 ] );
|
|
r [ 1 ] = B [ 1 ] - ( A [ 1 ] [ 0 ] * guess [ 0 ] + A [ 1 ] [ 1 ] * guess [ 1 ] + A [ 1 ] [ 2 ] * guess [ 2 ] );
|
|
r [ 2 ] = B [ 2 ] - ( A [ 2 ] [ 0 ] * guess [ 0 ] + A [ 2 ] [ 1 ] * guess [ 1 ] + A [ 2 ] [ 2 ] * guess [ 2 ] );
|
|
|
|
for ( i = 0; i < n; i++ )
|
|
{
|
|
guess [ 0 ] = guess [ 0 ] + c * r [ 0 ];
|
|
guess [ 1 ] = guess [ 1 ] + c * r [ 1 ];
|
|
guess [ 2 ] = guess [ 2 ] + c * r [ 2 ];
|
|
|
|
r [ 0 ] = B [ 0 ] - ( A [ 0 ] [ 0 ] * guess [ 0 ] + A [ 0 ] [ 1 ] * guess [ 1 ] + A [ 0 ] [ 2 ] * guess [ 2 ] );
|
|
r [ 1 ] = B [ 1 ] - ( A [ 1 ] [ 0 ] * guess [ 0 ] + A [ 1 ] [ 1 ] * guess [ 1 ] + A [ 1 ] [ 2 ] * guess [ 2 ] );
|
|
r [ 2 ] = B [ 2 ] - ( A [ 2 ] [ 0 ] * guess [ 0 ] + A [ 2 ] [ 1 ] * guess [ 1 ] + A [ 2 ] [ 2 ] * guess [ 2 ] );
|
|
}
|
|
}
|
|
/*
|
|
if ( guess [ 0 ] > store [ 0 ] + 1 || guess [ 0 ] < store [ 0 ] - 1 ||
|
|
guess [ 1 ] > store [ 1 ] + 1 || guess [ 1 ] < store [ 1 ] - 1 ||
|
|
guess [ 2 ] > store [ 2 ] + 1 || guess [ 2 ] < store [ 2 ] - 1 )
|
|
{
|
|
printf ( "water let point go from %f,%f,%f to %f,%f,%f\n",
|
|
store [ 0 ], store [ 1 ], store [ 2 ], guess [ 0 ], guess [ 1 ], guess [ 2 ] );
|
|
printf ( "A is %f,%f,%f %f,%f,%f %f,%f,%f\n", A [ 0 ] [ 0 ], A [ 0 ] [ 1 ], A [ 0 ] [ 2 ],
|
|
A [ 1 ] [ 0 ] , A [ 1 ] [ 1 ], A [ 1 ] [ 2 ], A [ 2 ] [ 0 ], A [ 2 ] [ 1 ], A [ 2 ] [ 2 ] );
|
|
printf ( "B is %f,%f,%f\n", B [ 0 ], B [ 1 ], B [ 2 ] );
|
|
printf ( "bounding box is %f,%f,%f to %f,%f,%f\n",
|
|
box->begin.x, box->begin.y, box->begin.z, box->end.x, box->end.y, box->end.z );
|
|
}
|
|
*/
|
|
}
|
|
|
|
float calcPoint ( float halfA[], float b[], float btb, float midpoint[], float rvalue[], BoundingBoxf *box, float *mat )
|
|
{
|
|
float newB [ 3 ];
|
|
float a [ 3 ] [ 3 ];
|
|
float inv [ 3 ] [ 3 ];
|
|
float w [ 3 ];
|
|
float u [ 3 ] [ 3 ];
|
|
|
|
a [ 0 ] [ 0 ] = halfA [ 0 ];
|
|
a [ 0 ] [ 1 ] = halfA [ 1 ];
|
|
a [ 0 ] [ 2 ] = halfA [ 2 ];
|
|
a [ 1 ] [ 1 ] = halfA [ 3 ];
|
|
a [ 1 ] [ 2 ] = halfA [ 4 ];
|
|
a [ 1 ] [ 0 ] = halfA [ 1 ];
|
|
a [ 2 ] [ 0 ] = halfA [ 2 ];
|
|
a [ 2 ] [ 1 ] = halfA [ 4 ];
|
|
a [ 2 ] [ 2 ] = halfA [ 5 ];
|
|
|
|
switch ( method )
|
|
{
|
|
case 0:
|
|
case 1:
|
|
case 2:
|
|
rvalue [ 0 ] = midpoint [ 0 ];
|
|
rvalue [ 1 ] = midpoint [ 1 ];
|
|
rvalue [ 2 ] = midpoint [ 2 ];
|
|
|
|
descent ( a, b, rvalue, box );
|
|
return calcError ( a, b, btb, rvalue );
|
|
break;
|
|
case 3:
|
|
matInverse ( a, midpoint, inv, w, u );
|
|
|
|
|
|
newB [ 0 ] = b [ 0 ] - a [ 0 ] [ 0 ] * midpoint [ 0 ] - a [ 0 ] [ 1 ] * midpoint [ 1 ] - a [ 0 ] [ 2 ] * midpoint [ 2 ];
|
|
newB [ 1 ] = b [ 1 ] - a [ 1 ] [ 0 ] * midpoint [ 0 ] - a [ 1 ] [ 1 ] * midpoint [ 1 ] - a [ 1 ] [ 2 ] * midpoint [ 2 ];
|
|
newB [ 2 ] = b [ 2 ] - a [ 2 ] [ 0 ] * midpoint [ 0 ] - a [ 2 ] [ 1 ] * midpoint [ 1 ] - a [ 2 ] [ 2 ] * midpoint [ 2 ];
|
|
|
|
rvalue [ 0 ] = inv [ 0 ] [ 0 ] * newB [ 0 ] + inv [ 1 ] [ 0 ] * newB [ 1 ] + inv [ 2 ] [ 0 ] * newB [ 2 ] + midpoint [ 0 ];
|
|
rvalue [ 1 ] = inv [ 0 ] [ 1 ] * newB [ 0 ] + inv [ 1 ] [ 1 ] * newB [ 1 ] + inv [ 2 ] [ 1 ] * newB [ 2 ] + midpoint [ 1 ];
|
|
rvalue [ 2 ] = inv [ 0 ] [ 2 ] * newB [ 0 ] + inv [ 1 ] [ 2 ] * newB [ 1 ] + inv [ 2 ] [ 2 ] * newB [ 2 ] + midpoint [ 2 ];
|
|
return calcError ( a, b, btb, rvalue );
|
|
break;
|
|
case 4:
|
|
method = 3;
|
|
calcPoint ( halfA, b, btb, midpoint, rvalue, box, mat );
|
|
method = 4;
|
|
/*
|
|
int rank;
|
|
float eqs [ 4 ] [ 4 ];
|
|
|
|
// form the square matrix
|
|
eqs [ 0 ] [ 0 ] = mat [ 0 ];
|
|
eqs [ 0 ] [ 1 ] = mat [ 1 ];
|
|
eqs [ 0 ] [ 2 ] = mat [ 2 ];
|
|
eqs [ 0 ] [ 3 ] = mat [ 3 ];
|
|
eqs [ 1 ] [ 1 ] = mat [ 4 ];
|
|
eqs [ 1 ] [ 2 ] = mat [ 5 ];
|
|
eqs [ 1 ] [ 3 ] = mat [ 6 ];
|
|
eqs [ 2 ] [ 2 ] = mat [ 7 ];
|
|
eqs [ 2 ] [ 3 ] = mat [ 8 ];
|
|
eqs [ 3 ] [ 3 ] = mat [ 9 ];
|
|
eqs [ 1 ] [ 0 ] = eqs [ 2 ] [ 0 ] = eqs [ 2 ] [ 1 ] = eqs [ 3 ] [ 0 ] = eqs [ 3 ] [ 1 ] = eqs [ 3 ] [ 2 ] = 0;
|
|
|
|
// compute the new QR decomposition and rank
|
|
rank = qr ( eqs );
|
|
|
|
method = 2;
|
|
calcPoint ( halfA, b, btb, midpoint, rvalue, box, mat );
|
|
method = 4;
|
|
|
|
if ( rank == 0 )
|
|
{
|
|
// it's zero, no equations
|
|
rvalue [ 0 ] = midpoint [ 0 ];
|
|
rvalue [ 1 ] = midpoint [ 1 ];
|
|
rvalue [ 2 ] = midpoint [ 2 ];
|
|
}
|
|
else
|
|
{
|
|
if ( rank == 1 )
|
|
{
|
|
// one equation, it's a plane
|
|
float temp = ( eqs [ 0 ] [ 0 ] * midpoint [ 0 ] + eqs [ 0 ] [ 1 ] * midpoint [ 1 ] + eqs [ 0 ] [ 2 ] * midpoint [ 2 ] - eqs [ 0 ] [ 3 ] ) /
|
|
( eqs [ 0 ] [ 0 ] * eqs [ 0 ] [ 0 ] + eqs [ 0 ] [ 1 ] * eqs [ 0 ] [ 1 ] + eqs [ 0 ] [ 2 ] * eqs [ 0 ] [ 2 ] );
|
|
|
|
rvalue [ 0 ] = midpoint [ 0 ] - temp * eqs [ 0 ] [ 0 ];
|
|
rvalue [ 1 ] = midpoint [ 1 ] - temp * eqs [ 0 ] [ 1 ];
|
|
rvalue [ 2 ] = midpoint [ 2 ] - temp * eqs [ 0 ] [ 2 ];
|
|
}
|
|
else
|
|
{
|
|
if ( rank == 2 )
|
|
{
|
|
// two equations, it's a line
|
|
float a, b, c, d, e, f, g;
|
|
|
|
// reduce back to upper triangular
|
|
qr ( eqs, 2, 0.000001f );
|
|
|
|
a = eqs [ 0 ] [ 0 ];
|
|
b = eqs [ 0 ] [ 1 ];
|
|
c = eqs [ 0 ] [ 2 ];
|
|
d = eqs [ 0 ] [ 3 ];
|
|
e = eqs [ 1 ] [ 1 ];
|
|
f = eqs [ 1 ] [ 2 ];
|
|
g = eqs [ 1 ] [ 3 ];
|
|
|
|
// solved using the equations
|
|
// ax + by + cz = d
|
|
// ey + fz = g
|
|
// minimize (x-px)^2 + (y-py)^2 + (z-pz)^2
|
|
if ( a > 0.000001f || a < -0.000001f )
|
|
{
|
|
if ( e > 0.00000f || e < -0.000001f )
|
|
{
|
|
rvalue [ 2 ] = ( -1 * b * d * e * f + ( a * a + b * b ) * g * f + c * e * ( d * e - b * g ) +
|
|
a * e * ( ( b * f - c * e ) * midpoint [ 0 ] - a * f * midpoint [ 1 ] + a * e * midpoint [ 2 ] ) ) /
|
|
( a * a * ( e * e + f * f ) + ( c * e - b * f ) * ( c * e - b * f ) );
|
|
rvalue [ 1 ] = ( g - f * rvalue [ 2 ] ) / e;
|
|
rvalue [ 0 ] = ( d - b * rvalue [ 1 ] - c * rvalue [ 2 ] ) / a;
|
|
}
|
|
else
|
|
{
|
|
// slightly degenerate case where e==0
|
|
rvalue [ 2 ] = g / f;
|
|
rvalue [ 1 ] = ( b * d * f - b * c * g - a * b * f * midpoint [ 0 ] + a * a * f * midpoint [ 1 ] ) /
|
|
( a * a * f + b * b * f );
|
|
rvalue [ 0 ] = ( d - b * rvalue [ 1 ] - c * rvalue [ 2 ] ) / a;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// degenerate case where a==0 so e == 0 (upper triangular)
|
|
|
|
rvalue [ 2 ] = g / f;
|
|
rvalue [ 1 ] = ( d - c * rvalue [ 2 ] ) / b;
|
|
rvalue [ 0 ] = midpoint [ 0 ];
|
|
}
|
|
|
|
}
|
|
else
|
|
{
|
|
// must be three equations or more now... solve using back-substitution
|
|
rvalue [ 2 ] = mat [ 8 ] / mat [ 7 ];
|
|
rvalue [ 1 ] = ( mat [ 6 ] - mat [ 5 ] * rvalue [ 2 ] ) / mat [ 4 ];
|
|
rvalue [ 0 ] = ( mat [ 3 ] - mat [ 2 ] * rvalue [ 2 ] - mat [ 1 ] * rvalue [ 1 ] ) / mat [ 0 ];
|
|
}
|
|
}
|
|
}
|
|
*/
|
|
|
|
float ret;
|
|
float tmp;
|
|
|
|
ret = mat [ 9 ] * mat [ 9 ];
|
|
|
|
tmp = mat [ 0 ] * rvalue [ 0 ] + mat [ 1 ] * rvalue [ 1 ] + mat [ 2 ] * rvalue [ 2 ] - mat [ 3 ];
|
|
ret += tmp * tmp;
|
|
|
|
tmp = mat [ 4 ] * rvalue [ 1 ] + mat [ 5 ] * rvalue [ 2 ] - mat [ 6 ];
|
|
ret += tmp * tmp;
|
|
|
|
tmp = mat [ 7 ] * rvalue [ 2 ] - mat [ 8 ];
|
|
ret += tmp * tmp;
|
|
|
|
return ret;
|
|
|
|
break;
|
|
case 5:
|
|
rvalue [ 0 ] = midpoint [ 0 ];
|
|
rvalue [ 1 ] = midpoint [ 1 ];
|
|
rvalue [ 2 ] = midpoint [ 2 ];
|
|
|
|
return calcError ( a, b, btb, rvalue );
|
|
}
|
|
|
|
return 0 ;
|
|
}
|
|
|