zydronium/SBSPSS
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Browse source
This commit is contained in:
Daveo 2000-12-04 16:58:10 +00:00
parent 2a24119501
commit 79caf039ef
19 changed files with 3844 additions and 0 deletions
Download patch file Download diff file Expand all files Collapse all files

124
Utils/Libs/Maths/MgcAppr3DLineFit.cpp Normal file
View file

@ -0,0 +1,124 @@
// Magic Software, Inc.
// http://www.magic-software.com
// Copyright (c) 2000, All Rights Reserved
//
// Source code from Magic Software is supplied under the terms of a license
// agreement and may not be copied or disclosed except in accordance with the
// terms of that agreement. The various license agreements may be found at
// the Magic Software web site. This file is subject to the license
//
// FREE SOURCE CODE
// http://www.magic-software.com/License/free.pdf
#include "MgcEigen.h"
#include "vector3.h"
//#include "MgcLinearSystem.h"
#include "MgcAppr3DLineFit.h"
//----------------------------------------------------------------------------
void MgcOrthogonalLineFit (int iQuantity, const Vector3* akPoint,
Vector3& rkOffset, Vector3& rkDirection)
{
// compute average of points
rkOffset = akPoint[0];
int i;
for (i = 1; i < iQuantity; i++)
rkOffset += akPoint[i];
real fInvQuantity = 1.0f/iQuantity;
rkOffset *= fInvQuantity;
// compute sums of products
real fSumXX = 0.0, fSumXY = 0.0, fSumXZ = 0.0;
real fSumYY = 0.0, fSumYZ = 0.0, fSumZZ = 0.0;
for (i = 0; i < iQuantity; i++)
{
Vector3 kDiff = akPoint[i] - rkOffset;
fSumXX += kDiff.x*kDiff.x;
fSumXY += kDiff.x*kDiff.y;
fSumXZ += kDiff.x*kDiff.z;
fSumYY += kDiff.y*kDiff.y;
fSumYZ += kDiff.y*kDiff.z;
fSumZZ += kDiff.z*kDiff.z;
}
// setup the eigensolver
MgcEigen kES(3);
kES.Matrix(0,0) = fSumYY+fSumZZ;
kES.Matrix(0,1) = -fSumXY;
kES.Matrix(0,2) = -fSumXZ;
kES.Matrix(1,0) = kES.Matrix(0,1);
kES.Matrix(1,1) = fSumXX+fSumZZ;
kES.Matrix(1,2) = -fSumYZ;
kES.Matrix(2,0) = kES.Matrix(0,2);
kES.Matrix(2,1) = kES.Matrix(1,2);
kES.Matrix(2,2) = fSumXX+fSumYY;
// compute eigenstuff, smallest eigenvalue is in last position
kES.DecrSortEigenStuff3();
// unit-length direction for best-fit line
rkDirection.x = kES.GetEigenvector(0,2);
rkDirection.y = kES.GetEigenvector(1,2);
rkDirection.z = kES.GetEigenvector(2,2);
}
//----------------------------------------------------------------------------
bool MgcOrthogonalLineFit (int iQuantity, const Vector3* akPoint,
const bool* abValid, Vector3& rkOffset, Vector3& rkDirection)
{
// compute average of points
rkOffset.Zero();
int i, iValidQuantity = 0;
for (i = 0; i < iQuantity; i++)
{
if ( abValid[i] )
{
rkOffset += akPoint[i];
iValidQuantity++;
}
}
if ( iValidQuantity == 0 )
return false;
real fInvQuantity = 1.0f/iQuantity;
rkOffset *= fInvQuantity;
// compute sums of products
real fSumXX = 0.0, fSumXY = 0.0, fSumXZ = 0.0;
real fSumYY = 0.0, fSumYZ = 0.0, fSumZZ = 0.0;
for (i = 0; i < iQuantity; i++)
{
if ( abValid[i] )
{
Vector3 kDiff = akPoint[i] - rkOffset;
fSumXX += kDiff.x*kDiff.x;
fSumXY += kDiff.x*kDiff.y;
fSumXZ += kDiff.x*kDiff.z;
fSumYY += kDiff.y*kDiff.y;
fSumYZ += kDiff.y*kDiff.z;
fSumZZ += kDiff.z*kDiff.z;
}
}
// setup the eigensolver
MgcEigen kES(3);
kES.Matrix(0,0) = fSumYY+fSumZZ;
kES.Matrix(0,1) = -fSumXY;
kES.Matrix(0,2) = -fSumXZ;
kES.Matrix(1,0) = kES.Matrix(0,1);
kES.Matrix(1,1) = fSumXX+fSumZZ;
kES.Matrix(1,2) = -fSumYZ;
kES.Matrix(2,0) = kES.Matrix(0,2);
kES.Matrix(2,1) = kES.Matrix(1,2);
kES.Matrix(2,2) = fSumXX+fSumYY;
// compute eigenstuff, smallest eigenvalue is in last position
kES.DecrSortEigenStuff3();
// unit-length direction for best-fit line
rkDirection.x = kES.GetEigenvector(0,2);
rkDirection.y = kES.GetEigenvector(1,2);
rkDirection.z = kES.GetEigenvector(2,2);
return true;
}
//----------------------------------------------------------------------------