Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Menu
Open sidebar
Thomas Pitiot
SocialAgents3D
Commits
e2acbb61
Commit
e2acbb61
authored
Nov 07, 2012
by
pitiot
Browse files
init
parent
6db3ac1c
Changes
9
Hide whitespace changes
Inline
Side-by-side
include/ShapeMatching/rigidXfComputation.h
0 → 100644
View file @
e2acbb61
#ifndef __RIGIDXFCOMPUTATION_09122008__
#define __RIGIDXFCOMPUTATION_09122008__
#include
"Geometry/matrix.h"
#include
"Geometry/vector_gen.h"
using
namespace
CGoGN
;
/** two ways to compute the minimal linear transformation between 2 sets of points*/
class
RigidComputation
{
public
:
/** SVD decomposition */
//Thanks to Frederic Larue
// extern gmtl::Matrix44f getRigidTransform( std::vector<Geom::Vec3f> from,std::vector<Geom::Vec3f> to, float time);
/** Jacobian rotation : to use with the covariance matrix*/
// http://books.google.com/books?id=weYj75E_t6MC&pg=PA263&lpg=PA263&dq=diagonalize+matrix+Jacobi+rotation+method&source=web&ots=LPY0V-udrx&sig=0mBoM5L9nH3KAdiunJKuGeVDW0s&hl=en&sa=X&oi=book_result&resnum=5&ct=result#PPA266,M1
// rotate a throught beta in pq-plane to set a[p][q]=0
// rotation stored in r whose columns are eigenvectors of a
void
rotate
(
Geom
::
Matrix33f
&
a
,
Geom
::
Matrix33f
&
r
,
int
p
,
int
q
);
float
maxoffdiag
(
Geom
::
Matrix33f
&
a
,
int
&
p
,
int
&
q
);
//final a is diagonal matrix of eigenvalues
//final r has eigenvectors in rows
void
jacrot
(
Geom
::
Matrix33f
&
a
,
Geom
::
Matrix33f
&
r
);
static
int
jacobiDiagonalization
(
Geom
::
Matrix33f
&
A
,
Geom
::
Matrix33f
&
V
);
};
#endif
include/ShapeMatching/shapeMatching.h
0 → 100644
View file @
e2acbb61
#ifndef SHAPEMATCHING_H
#define SHAPEMATCHING_H
#include
"Geometry/matrix.h"
#include
"Geometry/vector_gen.h"
#include
"env_map.h"
using
namespace
CGoGN
;
template
<
typename
PFP
>
class
ShapeMatchingGen
{
public:
ShapeMatchingGen
(
typename
PFP
::
MAP
&
map
,
VertexAttribute
<
VEC3
>&
position
,
std
::
vector
<
Dart
>
v_darts
,
typename
PFP
::
REAL
beta
)
:
m_map
(
map
),
m_position
(
position
),
m_darts
(
v_darts
),
m_beta
(
beta
)
{
this
->
goal
=
this
->
m_map
.
template
addAttribute
<
VEC3
,
VERTEX
>(
""
);
}
~
ShapeMatchingGen
()
{
if
(
goal
.
isValid
())
this
->
m_map
.
template
removeAttribute
<
VEC3
,
VERTEX
>(
this
->
goal
);
}
virtual
void
initialize
()
=
0
;
virtual
void
animate
()
=
0
;
void
computeAqqMatrix
()
{
this
->
aqq
.
zero
();
for
(
unsigned
int
i
=
0
;
i
<
this
->
q
.
size
()
;
i
++
)
{
this
->
aqq
(
0
,
0
)
+=
this
->
q
[
i
][
0
]
*
this
->
q
[
i
][
0
];
this
->
aqq
(
1
,
0
)
+=
this
->
q
[
i
][
0
]
*
this
->
q
[
i
][
1
];
this
->
aqq
(
2
,
0
)
+=
this
->
q
[
i
][
0
]
*
this
->
q
[
i
][
2
];
this
->
aqq
(
0
,
1
)
+=
this
->
q
[
i
][
1
]
*
this
->
q
[
i
][
0
];
this
->
aqq
(
1
,
1
)
+=
this
->
q
[
i
][
1
]
*
this
->
q
[
i
][
1
];
this
->
aqq
(
2
,
1
)
+=
this
->
q
[
i
][
1
]
*
this
->
q
[
i
][
2
];
this
->
aqq
(
0
,
2
)
+=
this
->
q
[
i
][
2
]
*
this
->
q
[
i
][
0
];
this
->
aqq
(
1
,
2
)
+=
this
->
q
[
i
][
2
]
*
this
->
q
[
i
][
1
];
this
->
aqq
(
2
,
2
)
+=
this
->
q
[
i
][
2
]
*
this
->
q
[
i
][
2
];
}
this
->
aqq
.
invert
(
this
->
aqq
);
}
VEC3
computeBarycenterDarts
()
{
VEC3
b
(
0
);
for
(
std
::
vector
<
Dart
>::
iterator
it
=
this
->
m_darts
.
begin
()
;
it
!=
this
->
m_darts
.
end
()
;
++
it
)
b
+=
this
->
m_position
[
*
it
];
b
/=
this
->
m_darts
.
size
();
return
b
;
}
typename
PFP
::
REAL
computeDeterminant
(
const
Geom
::
Matrix33f
&
a
)
{
return
a
(
0
,
0
)
*
a
(
1
,
1
)
*
a
(
2
,
2
)
+
a
(
0
,
2
)
*
a
(
1
,
0
)
*
a
(
2
,
1
)
+
a
(
0
,
1
)
*
a
(
1
,
2
)
*
a
(
2
,
0
)
-
a
(
0
,
2
)
*
a
(
1
,
1
)
*
a
(
2
,
0
)
-
a
(
0
,
0
)
*
a
(
1
,
2
)
*
a
(
2
,
1
)
-
a
(
0
,
1
)
*
a
(
1
,
0
)
*
a
(
2
,
2
);
}
// LIST_MAT m_lm;
//
typename
PFP
::
MAP
&
m_map
;
VertexAttribute
<
VEC3
>&
m_position
;
std
::
vector
<
Dart
>
m_darts
;
VertexAttribute
<
VEC3
>
goal
;
Geom
::
Matrix33f
aqq
;
std
::
vector
<
Geom
::
Vec3f
>
q
;
typename
PFP
::
REAL
m_beta
;
};
#endif
include/ShapeMatching/shapeMatchingLinear.h
0 → 100644
View file @
e2acbb61
#ifndef SHAPEMATCHING_LINEAR_H
#define SHAPEMATCHING_LINEAR_H
#include
"shapeMatching.h"
using
namespace
CGoGN
;
template
<
typename
PFP
>
class
ShapeMatchingLinear
:
public
ShapeMatchingGen
<
PFP
>
{
public:
ShapeMatchingLinear
(
typename
PFP
::
MAP
&
map
,
VertexAttribute
<
VEC3
>&
position
,
std
::
vector
<
Dart
>
v_darts
,
typename
PFP
::
REAL
beta
);
~
ShapeMatchingLinear
();
void
initialize
();
void
animate
();
protected:
VertexAttribute
<
VEC3
>
vecToBary
;
};
#include
"shapeMatchingLinear.hpp"
#endif
include/ShapeMatching/shapeMatchingLinear.hpp
0 → 100644
View file @
e2acbb61
#include
"rigidXfComputation.h"
//******************************************************************************
template
<
typename
PFP
>
ShapeMatchingLinear
<
PFP
>::
ShapeMatchingLinear
(
typename
PFP
::
MAP
&
map
,
VertexAttribute
<
VEC3
>&
position
,
std
::
vector
<
Dart
>
v_darts
,
typename
PFP
::
REAL
beta
)
:
ShapeMatchingGen
<
PFP
>
(
map
,
position
,
v_darts
,
beta
)
{
this
->
vecToBary
=
this
->
m_map
.
template
addAttribute
<
VEC3
,
VERTEX
>(
""
);
}
//******************************************************************************
template
<
typename
PFP
>
ShapeMatchingLinear
<
PFP
>::~
ShapeMatchingLinear
()
{
std
::
cout
<<
"delete SM"
<<
std
::
endl
;
if
(
this
->
vecToBary
.
isValid
())
this
->
m_map
.
template
removeAttribute
<
VEC3
,
VERTEX
>(
this
->
vecToBary
);
}
template
<
typename
PFP
>
void
ShapeMatchingLinear
<
PFP
>::
initialize
()
{
assert
(
this
->
m_darts
.
size
()
>
0
);
typedef
typename
PFP
::
VEC3
VEC3
;
typedef
typename
PFP
::
REAL
REAL
;
//********************* first compute the data for the MATS***********************************************************
//barycenter
VEC3
x0cm
=
this
->
computeBarycenterDarts
();
this
->
q
.
reserve
(
this
->
m_darts
.
size
());
for
(
std
::
vector
<
Dart
>::
iterator
it
=
this
->
m_darts
.
begin
();
it
!=
this
->
m_darts
.
end
();
++
it
)
{
VEC3
myQ
(
this
->
m_position
[
*
it
]
-
x0cm
);
this
->
q
.
push_back
(
myQ
);
}
this
->
computeAqqMatrix
();
//********************* then compute the vector to barycenter for the map***********************************************************
//compute barycenter
// VEC3 b = this->computeBarycenterDarts();
for
(
std
::
vector
<
Dart
>::
iterator
it
=
this
->
m_darts
.
begin
()
;
it
!=
this
->
m_darts
.
end
()
;
++
it
)
this
->
vecToBary
[
*
it
]
=
this
->
m_position
[
*
it
]
-
x0cm
;
}
template
<
typename
PFP
>
void
ShapeMatchingLinear
<
PFP
>::
animate
()
{
typedef
typename
PFP
::
VEC3
VEC3
;
//initialize(nfe,frame_prec);
unsigned
int
n
=
m_darts
.
size
();
VEC3
xcm
=
this
->
computeBarycenterDarts
();
std
::
vector
<
VEC3
>
p
;
p
.
reserve
(
n
);
for
(
std
::
vector
<
Dart
>::
iterator
it
=
this
->
m_darts
.
begin
();
it
!=
this
->
m_darts
.
end
();
++
it
)
p
.
push_back
(
this
->
m_position
[
*
it
]
-
xcm
);
Geom
::
Matrix33f
apq
;
apq
.
zero
();
for
(
unsigned
int
i
=
0
;
i
<
p
.
size
();
++
i
)
for
(
unsigned
int
x
=
0
;
x
<
3
;
++
x
)
for
(
unsigned
int
y
=
0
;
y
<
3
;
++
y
)
apq
(
x
,
y
)
+=
p
[
i
][
x
]
*
this
->
q
[
i
][
y
];
Geom
::
Matrix33f
s
=
apq
.
transposed
()
*
apq
;
Geom
::
Matrix33f
sr
;
RigidComputation
::
jacobiDiagonalization
(
s
,
sr
);
s
(
0
,
0
)
=
1.0
f
/
sqrt
(
s
(
0
,
0
));
s
(
1
,
1
)
=
1.0
f
/
sqrt
(
s
(
1
,
1
));
s
(
2
,
2
)
=
1.0
f
/
sqrt
(
s
(
2
,
2
));
s
=
sr
*
s
*
sr
.
transposed
();
Geom
::
Matrix33f
r
=
apq
*
s
;
Geom
::
Matrix33f
a
=
apq
*
this
->
aqq
;
typename
PFP
::
REAL
det
=
this
->
computeDeterminant
(
a
);
typename
PFP
::
REAL
detAInv
=
1.0
f
/
powf
(
det
,
1.0
f
/
3.0
f
);
if
(
std
::
isfinite
(
detAInv
))
{
for
(
unsigned
int
x
=
0
;
x
<
3
;
++
x
)
for
(
unsigned
int
y
=
0
;
y
<
3
;
++
y
)
r
(
x
,
y
)
=
this
->
m_beta
*
a
(
x
,
y
)
*
detAInv
+
(
1.0
f
-
this
->
m_beta
)
*
r
(
x
,
y
);
}
else
{
std
::
cout
<<
"no linear deformation (planar config ?), applying identity"
<<
std
::
endl
;
r
.
identity
();
}
//once the transformation computed : move the vertices of the map
//compute barycenter
VEC3
b
=
this
->
computeBarycenterDarts
();
//move the vertices
for
(
std
::
vector
<
Dart
>::
iterator
it
=
this
->
m_darts
.
begin
()
;
it
!=
this
->
m_darts
.
end
()
;
++
it
)
{
this
->
goal
[
*
it
]
=
r
*
this
->
vecToBary
[
*
it
]
+
xcm
;
}
}
include/ShapeMatching/shapeMatchingQuadratic.h
0 → 100644
View file @
e2acbb61
#ifndef SHAPEMATCHING_QUAD_H
#define SHAPEMATCHING_QUAD_H
#include
"shapeMatching.h"
#include
"Geometry/vector_gen.h"
using
namespace
CGoGN
;
template
<
typename
PFP
>
class
ShapeMatchingQuadratic
:
public
ShapeMatchingGen
<
PFP
>
{
public:
ShapeMatchingQuadratic
(
typename
PFP
::
MAP
&
map
,
VertexAttribute
<
VEC3
>&
position
,
std
::
vector
<
Dart
>
v_darts
,
typename
PFP
::
REAL
beta
);
~
ShapeMatchingQuadratic
();
void
initialize
();
void
animate
();
protected:
Geom
::
Matrix
<
9
,
9
,
typename
PFP
::
REAL
>
aqqQuad
;
//vectors to barycenter
std
::
vector
<
Geom
::
Vector
<
9
,
typename
PFP
::
REAL
>
>
qQuad
;
VertexAttribute
<
Geom
::
Vector
<
9
,
typename
PFP
::
REAL
>
>
vecQuad
;
};
#include
"shapeMatchingQuadratic.hpp"
#endif
include/ShapeMatching/shapeMatchingQuadratic.hpp
0 → 100644
View file @
e2acbb61
#include
"rigidXfComputation.h"
using
namespace
CGoGN
;
//******************************************************************************
template
<
typename
PFP
>
ShapeMatchingQuadratic
<
PFP
>::
ShapeMatchingQuadratic
(
typename
PFP
::
MAP
&
map
,
VertexAttribute
<
VEC3
>&
position
,
std
::
vector
<
Dart
>
v_darts
,
typename
PFP
::
REAL
beta
)
:
ShapeMatchingGen
<
PFP
>
(
map
,
position
,
v_darts
,
beta
)
{
this
->
vecQuad
=
this
->
m_map
.
template
addAttribute
<
Geom
::
Vector
<
9
,
typename
PFP
::
REAL
>,
VERTEX
>
(
""
);
}
//******************************************************************************
template
<
typename
PFP
>
ShapeMatchingQuadratic
<
PFP
>::~
ShapeMatchingQuadratic
()
{
if
(
this
->
vecQuad
.
isValid
())
this
->
m_map
.
template
removeAttribute
<
Geom
::
Vector
<
9
,
typename
PFP
::
REAL
>
>
(
this
->
vecQuad
);
}
template
<
typename
PFP
>
void
ShapeMatchingQuadratic
<
PFP
>::
initialize
()
{
assert
(
this
->
m_darts
.
size
()
>
0
);
typedef
typename
PFP
::
VEC3
VEC3
;
typedef
typename
PFP
::
REAL
REAL
;
//********************* first compute the data for the MATS***********************************************************
//barycenter
VEC3
x0cm
=
this
->
computeBarycenterDarts
();
this
->
q
.
reserve
(
this
->
m_darts
.
size
());
for
(
std
::
vector
<
Dart
>::
iterator
it
=
this
->
m_darts
.
begin
();
it
!=
this
->
m_darts
.
end
();
++
it
)
{
VEC3
myQ
(
this
->
m_position
[
*
it
]
-
x0cm
);
this
->
q
.
push_back
(
myQ
);
Geom
::
Vector
<
9
,
typename
PFP
::
REAL
>
myQQuad
;
myQQuad
[
0
]
=
myQ
[
0
];
myQQuad
[
1
]
=
myQ
[
1
];
myQQuad
[
2
]
=
myQ
[
2
];
myQQuad
[
3
]
=
myQ
[
0
]
*
myQ
[
0
];
myQQuad
[
4
]
=
myQ
[
1
]
*
myQ
[
1
];
myQQuad
[
5
]
=
myQ
[
2
]
*
myQ
[
2
];
myQQuad
[
6
]
=
myQ
[
0
]
*
myQ
[
1
];
myQQuad
[
7
]
=
myQ
[
1
]
*
myQ
[
2
];
myQQuad
[
8
]
=
myQ
[
2
]
*
myQ
[
0
];
qQuad
.
push_back
(
myQQuad
);
}
this
->
computeAqqMatrix
();
aqqQuad
.
zero
();
for
(
unsigned
int
i
=
0
;
i
<
this
->
q
.
size
();
++
i
)
for
(
int
x
=
0
;
x
<
9
;
++
x
)
for
(
int
y
=
0
;
y
<
9
;
++
y
)
aqqQuad
(
x
,
y
)
+=
qQuad
[
i
][
x
]
*
qQuad
[
i
][
y
];
aqqQuad
.
invert
(
aqqQuad
);
//********************* then compute the vector to barycenter for the map***********************************************************
//compute barycenter
// VEC3 b = this->computeBarycenterDarts();
for
(
std
::
vector
<
Dart
>::
iterator
it
=
this
->
m_darts
.
begin
()
;
it
!=
this
->
m_darts
.
end
()
;
++
it
)
{
VEC3
toBary
=
this
->
m_position
[
*
it
]
-
x0cm
;
Geom
::
Vector
<
9
,
typename
PFP
::
REAL
>
myQQuad
;
myQQuad
[
0
]
=
toBary
[
0
];
myQQuad
[
1
]
=
toBary
[
1
];
myQQuad
[
2
]
=
toBary
[
2
];
myQQuad
[
3
]
=
toBary
[
0
]
*
toBary
[
0
];
myQQuad
[
4
]
=
toBary
[
1
]
*
toBary
[
1
];
myQQuad
[
5
]
=
toBary
[
2
]
*
toBary
[
2
];
myQQuad
[
6
]
=
toBary
[
0
]
*
toBary
[
1
];
myQQuad
[
7
]
=
toBary
[
1
]
*
toBary
[
2
];
myQQuad
[
8
]
=
toBary
[
2
]
*
toBary
[
0
];
this
->
vecQuad
[
*
it
]
=
myQQuad
;
}
}
template
<
typename
PFP
>
void
ShapeMatchingQuadratic
<
PFP
>::
animate
()
{
typedef
typename
PFP
::
VEC3
VEC3
;
unsigned
int
n
=
this
->
m_darts
.
size
();
VEC3
xcm
=
this
->
computeBarycenterDarts
();
std
::
vector
<
VEC3
>
p
;
p
.
reserve
(
n
);
Geom
::
Matrix
<
3
,
9
,
typename
PFP
::
REAL
>
apqQuad
;
apqQuad
.
zero
();
Geom
::
Matrix33f
apq
;
apq
.
zero
();
unsigned
int
i
=
0
;
for
(
std
::
vector
<
Dart
>::
iterator
it
=
this
->
m_darts
.
begin
();
it
!=
this
->
m_darts
.
end
();
++
it
,
++
i
)
{
p
.
push_back
(
this
->
m_position
[
*
it
]
-
xcm
);
for
(
unsigned
int
x
=
0
;
x
<
3
;
x
++
)
for
(
unsigned
int
y
=
0
;
y
<
9
;
y
++
)
{
if
(
y
<
3
)
apq
(
x
,
y
)
+=
p
[
i
][
x
]
*
this
->
q
[
i
][
y
];
apqQuad
(
x
,
y
)
+=
p
[
i
][
x
]
*
qQuad
[
i
][
y
];
}
}
Geom
::
Matrix33f
s
=
apq
.
transposed
()
*
apq
;
Geom
::
Matrix33f
sr
;
RigidComputation
::
jacobiDiagonalization
(
s
,
sr
);
s
(
0
,
0
)
=
1.0
f
/
sqrt
(
s
(
0
,
0
));
s
(
1
,
1
)
=
1.0
f
/
sqrt
(
s
(
1
,
1
));
s
(
2
,
2
)
=
1.0
f
/
sqrt
(
s
(
2
,
2
));
s
=
sr
*
s
*
sr
.
transposed
();
Geom
::
Matrix33f
r
=
apq
*
s
;
//compute R tilde
Geom
::
Matrix
<
3
,
9
,
typename
PFP
::
REAL
>
r_big
;
r_big
.
zero
();
for
(
unsigned
int
x
=
0
;
x
<
3
;
++
x
)
for
(
unsigned
int
y
=
0
;
y
<
3
;
++
y
)
r_big
(
x
,
y
)
=
r
(
x
,
y
);
//compute quadratic deformation
//#define BETA 0.5f
Geom
::
Matrix
<
3
,
9
,
typename
PFP
::
REAL
>
a
=
apqQuad
*
this
->
aqqQuad
;
Geom
::
Matrix
<
9
,
9
,
typename
PFP
::
REAL
>
a_square
;
a_square
.
identity
();
for
(
unsigned
int
x
=
0
;
x
<
3
;
++
x
)
for
(
unsigned
int
y
=
0
;
y
<
9
;
++
y
)
a_square
(
x
,
y
)
=
a
(
x
,
y
);
typename
PFP
::
REAL
det
=
a_square
.
invert
(
a_square
);
det
=
1.0
f
/
pow
(
fabs
(
det
),
1.0
f
/
9.0
f
);
if
(
det
<
0.0
f
)
det
=
-
det
;
a
*=
det
;
//Geom::Matrix<3,9,typename PFP::REAL> t = BETA*a+(1.0f-BETA)*r_big;
Geom
::
Matrix
<
3
,
9
,
typename
PFP
::
REAL
>
t
=
this
->
m_beta
*
a
+
(
1.0
f
-
this
->
m_beta
)
*
r_big
;
//once the transformation computed : move the vertices of the map
//compute barycenter
// VEC3 b = this->computeBarycenterDarts();
// std::cout << "b " << b << std::endl;
//move the vertices
for
(
std
::
vector
<
Dart
>::
iterator
it
=
this
->
m_darts
.
begin
()
;
it
!=
this
->
m_darts
.
end
()
;
++
it
)
{
Geom
::
Vector
<
3
,
typename
PFP
::
REAL
>
q_d
=
t
*
this
->
vecQuad
[
*
it
];
typename
PFP
::
VEC3
v
(
q_d
[
0
],
q_d
[
1
],
q_d
[
2
]);
this
->
goal
[
*
it
]
=
v
+
xcm
;
}
}
include/moving_mesh.h
0 → 100644
View file @
e2acbb61
#ifndef M_MOVING_MESH_H
#define M_MOVING_MESH_H
#include
<iostream>
#include
"utils.h"
#include
"env_map.h"
#include
"moving_obstacle.h"
#include
"Algo/Import/import.h"
#include
"ShapeMatching/shapeMatchingQuadratic.h"
using
namespace
std
;
class
MovingMesh
{
public:
MovingMesh
(
EnvMap
*
envMap
,
Dart
d
,
std
::
string
filename
);
void
linkWithObstacle
(
MovingObstacle
*
mo
);
void
scale
(
float
val
);
void
moveInFace
(
typename
PFP
::
MAP
&
motherMap
,
Dart
d
,
VertexAttribute
<
VEC3
>
pos
);
void
move
();
void
animate
();
std
::
vector
<
VEC3
>
computeProjectedPointSet
();
std
::
vector
<
VEC3
>
jarvisConvexHull
(
std
::
vector
<
VEC3
>
projectedPointSet
);
std
::
vector
<
VEC3
>
computeSkeleton
(
std
::
vector
<
VEC3
>
projectedPointSet
,
unsigned
int
nodeNumber
);
typename
PFP
::
MAP
map
;
VertexAttribute
<
VEC3
>
position
;
VertexAttribute
<
VEC3
>
normal
;
ShapeMatchingQuadratic
<
PFP
>
*
smg
;
std
::
vector
<
VEC3
>
skeleton
;
MovingObstacle
*
mo
;
EnvMap
*
envMap_
;
CellMarker
<
VERTEX
>
constrainedV
;
};
#endif
src/ShapeMatching/rigidXfComputation.cpp
0 → 100644
View file @
e2acbb61
#include
"ShapeMatching/rigidXfComputation.h"
using
namespace
CGoGN
;
// gmtl::Matrix44f getRigidTransform( std::vector<Geom::Vec3f> from, std::vector<Geom::Vec3f> to,float time)
// {
// const int nb_c = from.size();
//
// /* Compute the centroids of both datasets */
// Geom::Vec3f mu1( 0.0f, 0.0f, 0.0f );
// Geom::Vec3f mu2( 0.0f, 0.0f, 0.0f );
//
// for( int i=0; i<nb_c; i++ )
// {
// mu1 += to[i];
// mu2 += from[i];
// }
//
// mu1 /= (float) nb_c;
// mu2 /= (float) nb_c;
//
//
// /* Compute the covariance matrix */
// Matrix33d S;
// S.setNull();
//
// for( int i=0; i<nb_c; i++ )
// {
// gmtl::Vec3f p1 = to[i] - mu1;
// gmtl::Vec3f p2 = from[i] - mu2;
//
// S[0][0] += p2[0] * p1[0];
// S[0][1] += p2[0] * p1[1];
// S[0][2] += p2[0] * p1[2];
//
// S[1][0] += p2[1] * p1[0];
// S[1][1] += p2[1] * p1[1];
// S[1][2] += p2[1] * p1[2];
//
// S[2][0] += p2[2] * p1[0];
// S[2][1] += p2[2] * p1[1];
// S[2][2] += p2[2] * p1[2];
// }
//
// S /= (REAL) nb_c;
//
//
// /* Build the symetric matrix N minimizing the error by quaternion rotation */
// MatrixNMd N( 4, 4 );
//
// N[0][0] = S.at(0,0) + S.at(1,1) + S.at(2,2);
// N[0][1] = S.at(1,2) - S.at(2,1);
// N[0][2] = S.at(2,0) - S.at(0,2);
// N[0][3] = S.at(0,1) - S.at(1,0);
//
// N[1][0] = N.at(0,1);
// N[1][1] = S.at(0,0) - S.at(1,1) - S.at(2,2);
// N[1][2] = S.at(0,1) + S.at(1,0);
// N[1][3] = S.at(2,0) + S.at(0,2);
//
// N[2][0] = N.at(0,2);
// N[2][1] = N.at(1,2);
// N[2][2] = S.at(1,1) - S.at(0,0) - S.at(2,2);
// N[2][3] = S.at(1,2) + S.at(2,1);
//
// N[3][0] = N.at(0,3);
// N[3][1] = N.at(1,3);
// N[3][2] = N.at(2,3);
// N[3][3] = S.at(2,2) - S.at(0,0) - S.at(1,1);
//
//