/*!
* \return Product of the two given quaternions.
* \ingroup HGU_GL
* \brief Calculates and returns the product of the two given
* quaternions.
* \param given1 First quaternion.
* \param given2 Second quaternion.
*/
HGUglQuaternion HGUglQuatProduct(HGUglQuaternion given1,
HGUglQuaternion given2)
{
HGUglQuaternion prod;
WlzDVertex3 tVtx0,
tVtx1,
tVtx2;
prod.qW = (given1.qW * given2.qW) - WLZ_VTX_3_DOT(given1.qV, given2.qV);
WLZ_VTX_3_SCALE(tVtx0, given2.qV, given1.qW);
WLZ_VTX_3_SCALE(tVtx1, given1.qV, given2.qW);
WLZ_VTX_3_CROSS(tVtx2, given1.qV, given2.qV);
WLZ_VTX_3_ADD3(prod.qV, tVtx0, tVtx1, tVtx2);
return(prod);
}
/*!
* \return Coordinates of centre of mass.
* \ingroup WlzFeatures
* \brief Computes the centre of mass of a vector of 3D vertices.
* \param nVtx Number of vertices.
* \param vtx The vertices.
*/
WlzDVertex3 WlzCentreOfMassVtx3D(int nVtx, WlzDVertex3 *vtx)
{
WlzDVertex3 cen;
WLZ_VTX_3_ZERO(cen);
if(nVtx > 0)
{
int idx;
for(idx = 0; idx < nVtx; ++idx)
{
WLZ_VTX_3_ADD(cen, cen, vtx[idx]);
}
WLZ_VTX_3_SCALE(cen, cen, 1.0 / nVtx);
}
return(cen);
}
/*!
* \return Calculated quaternion.
* \ingroup HGU_GL
* \brief Calculates a rotation quaternion (which lies on the unit
* sphere) using the given axis and rotation about that axis.
* \param axis Axis defined by vector throgh
* (0, 0, 0) and this vertex.
* \param phi Rotation about the given axis.
*/
HGUglQuaternion HGUglQuatFromAxis(WlzDVertex3 axis, double phi)
{
double halfPhi,
axisLen,
tD1;
HGUglQuaternion quat;
halfPhi = phi / 2.0;
axisLen = WLZ_VTX_3_LENGTH(axis);
if(axisLen > DBL_EPSILON)
{
quat.qW = cos(halfPhi);
tD1 = sin(halfPhi) / axisLen;
WLZ_VTX_3_SCALE(quat.qV, axis, tD1);
}
else
{
HGUgl_QUAT_SET4(quat, 1.0, 0.0, 0.0, 0.0);
}
return(quat);
}
/*!
* \return Woolz error code.
* \ingroup WlzTransform
* \brief Fits a plane to the given vertices using singular value
* decomposition. The best fit plane is returned (provided
* there is no error) as a unit normal \f$n\f$ and the
* centroid of the given vertices ( \f$r_0\f$ ) which in a
* point in the plane, with the plane equation
* \f[
\vec{n}\cdot(\vec{r} - \vec{r_0}) = \vec{0}
\f]
* \param vtxType Given vertex type.
* \param nVtx Number of given vertices.
* \param vtx Given vertices.
* \param dstPinP Destination pointer for the return
* of the median point (\f$r_0\f$) in
* the plane.
* \param dstNrm Destination pointer for the unit
* normal to the plane (\f$n\f$).
*/
WlzErrorNum WlzFitPlaneSVD(WlzVertexType vtxType, int nVtx, WlzVertexP vtx,
WlzDVertex3 *dstPinP, WlzDVertex3 *dstNrm)
{
AlgMatrix aMat,
vMat;
WlzDVertex3 centroid,
normal;
double *sVec = NULL;
AlgError algErr = ALG_ERR_NONE;
WlzErrorNum errNum = WLZ_ERR_NONE;
aMat.core = NULL;
vMat.core = NULL;
if(nVtx < 0)
{
errNum = WLZ_ERR_PARAM_DATA;
}
else if(vtx.v == NULL)
{
errNum = WLZ_ERR_PARAM_NULL;
}
if(errNum == WLZ_ERR_NONE)
{
if(((sVec = (double *)AlcCalloc(sizeof(double), 3)) == NULL) ||
((aMat.rect = AlgMatrixRectNew(nVtx, 3, NULL)) == NULL) ||
((vMat.rect = AlgMatrixRectNew(3, 3, NULL)) == NULL))
{
errNum = WLZ_ERR_MEM_ALLOC;
}
}
/* Compute a Nx3 matrix from the given vertices then subtract their
* centroid. */
if(errNum == WLZ_ERR_NONE)
{
int i;
double *row;
WLZ_VTX_3_ZERO(centroid);
switch(vtxType)
{
case WLZ_VERTEX_I3:
for(i = 0; i < nVtx; ++i)
{
WlzIVertex3 *v;
v = vtx.i3 + i;
row = aMat.rect->array[i];
centroid.vtX += row[0] = v->vtX;
centroid.vtY += row[1] = v->vtY;
centroid.vtZ += row[2] = v->vtZ;
}
break;
case WLZ_VERTEX_F3:
for(i = 0; i < nVtx; ++i)
{
WlzFVertex3 *v;
v = vtx.f3 + i;
row = aMat.rect->array[i];
centroid.vtX += row[0] = v->vtX;
centroid.vtY += row[1] = v->vtY;
centroid.vtZ += row[2] = v->vtZ;
}
break;
case WLZ_VERTEX_D3:
for(i = 0; i < nVtx; ++i)
{
WlzDVertex3 *v;
v = vtx.d3 + i;
row = aMat.rect->array[i];
centroid.vtX += row[0] = v->vtX;
centroid.vtY += row[1] = v->vtY;
centroid.vtZ += row[2] = v->vtZ;
}
break;
default:
errNum = WLZ_ERR_PARAM_TYPE;
break;
}
if(errNum == WLZ_ERR_NONE)
{
WLZ_VTX_3_SCALE(centroid, centroid, (1.0 / nVtx));
for(i = 0; i < nVtx; ++i)
{
row = aMat.rect->array[i];
row[0] -= centroid.vtX;
row[1] -= centroid.vtY;
row[2] -= centroid.vtZ;
}
}
}
/* Compute SVD. */
if(errNum == WLZ_ERR_NONE)
{
algErr = AlgMatrixSVDecomp(aMat, sVec, vMat);
errNum = WlzErrorFromAlg(algErr);
}
/* Find the vector corresponding to the least singular value. */
if(errNum == WLZ_ERR_NONE)
{
int i,
m;
double len;
double **ary;
const double eps = 1.0e-06;
m = 0;
for(i = 1; i < 3; ++i)
{
if(sVec[i] < sVec[m])
{
m = i;
}
}
ary = vMat.rect->array;
#ifdef WLZ_FITPLANESVD_DEBUG
(void )fprintf(stderr,
"WlzFitPlaneSVD() singular values = {%lg, %lg, %lg}\n",
sVec[0], sVec[1], sVec[2]);
(void )fprintf(stderr,
"WlzFitPlaneSVD() index of min singular value = %d\n",
m);
(void )fprintf(stderr,
"WlzFitPlaneSVD() singular vectors = \n"
"{\n");
for(i = 0; i < 3; ++i)
{
(void )fprintf(stderr,
" {%lg, %lg, %lg}\n",
ary[i][0], ary[i][1], ary[i][2]);
}
(void )fprintf(stderr,
"}\n");
#endif /* WLZ_FITPLANESVD_DEBUG */
normal.vtX = ary[0][m];
normal.vtY = ary[1][m];
normal.vtZ = ary[2][m];
len = WLZ_VTX_3_LENGTH(normal);
if(len < eps)
{
errNum = WLZ_ERR_ALG_SINGULAR;
}
else
{
WLZ_VTX_3_SCALE(normal, normal, (1.0 / len));
}
}
AlgMatrixRectFree(aMat.rect);
AlgMatrixRectFree(vMat.rect);
AlcFree(sVec);
if(errNum == WLZ_ERR_NONE)
{
if(dstNrm)
{
*dstNrm = normal;
}
if(dstPinP)
{
*dstPinP = centroid;
}
}
return(errNum);
}
/*! input is the voxsel sip and the unit vector nStraghtline, indicating direction
in OPT space.
output is the *sip1p in view space!
*/
static WlzErrorNum WlzGetCorssPoint( WlzVertex sip,
WlzVertex nStraghtline,
WlzVertex *sip1p,
WlzThreeDViewStruct *wlzViewStri
)
{
WlzVertex vtemp, vtemp0, vo, vso, vtx1;
WlzVertex vs1, nI;
double dtemp, dtemp1, dtemp2;
WlzErrorNum errNum = WLZ_ERR_NONE;
/* output for test */
/*
printf("Yaw: %f\n", wlzViewStri->theta*180.0/3.14);
printf("Pitch: %f\n", wlzViewStri->phi*180.0/3.14 );
printf("Roll: %f\n", wlzViewStri->zeta*180.0/3.14 );
printf("distance: %f\n", wlzViewStri->dist );
printf("scaling: %f\n", wlzViewStri->scale );
printf("%f %f %f\n", wlzViewStri->fixed.vtX, wlzViewStri->fixed.vtY ,wlzViewStri->fixed.vtZ );
*/
vtx1.d3.vtX = 0.0;
vtx1.d3.vtY = 0.0;
vtx1.d3.vtZ = 1.0;
/* get the normal n_i = nI.d3 of this plan i -plane do not need !!! */
errNum = Wlz3DSectionTransformInvVtxR(wlzViewStri, vtx1.d3, &nI.d3 );
/* in OPT space */
/* check the n_{i} * nStraghtline != 0 other wise throw an erro */
dtemp = WLZ_VTX_3_DOT(nStraghtline.d3, nI.d3);
/* dtemp = abs(dtemp); */
if( ( dtemp < 0.000001 ) && ( dtemp > -0.000001 ) )
{
printf("%lg\n",dtemp);
printf(" the n_{i} is verticl to n_straght line in OPT space,\n");
printf(" there is no crossed point. Please chose different straight\n");
printf("line and try again\n ");
printf("%lg %lg %lg\n", nI.d3.vtX, nI.d3.vtY, nI.d3.vtZ);
printf("%lg %lg %lg\n", nStraghtline.d3.vtX, nStraghtline.d3.vtY, nStraghtline.d3.vtZ);
exit( 1 );
}
else
{
/* not parallel to z-direction */
/* s_{i} = x_{i} + n_straight_line * s
= x_{i} + n_straight_line *
< [ O_{i} - x_{i} ] dot n_{i} / ( n_straightline dot n_{i} ) >
*/
/* dtemp = ( n_straightlien dot n_{i} ) aleady got */
/* get the o_i+1
vo.d3.vtX = wlzViewStr1->fixed.vtX + wlzViewStr->dist * nI.d3.vtX;
vo.d3.vtY = wlzViewStr1->fixed.vtY + wlzViewStr->dist * nI.d3.vtY;
vo.d3.vtY = wlzViewStr1->fixed.vtZ + wlzViewStr->dist * nI.d3.vtZ;
*/
WLZ_VTX_3_SCALE(vtemp0.d3, nI.d3, wlzViewStri->dist);
WLZ_VTX_3_ADD(vo.d3, wlzViewStri->fixed, vtemp0.d3);
/* vso.d3 = vo.d3 - vx.d3 */
WLZ_VTX_3_SUB(vso.d3,vo.d3,sip.d3);
/* vs0.d3 dot n_i */
dtemp1 = WLZ_VTX_3_DOT(vso.d3, nI.d3);
/* get the s */
dtemp2 = dtemp1 / dtemp;
/* vtemp.d3 = ( n_straightlien * s ) */
WLZ_VTX_3_SCALE(vtemp.d3, nStraghtline.d3, dtemp2 );
/* now get the p_{i} */
WLZ_VTX_3_ADD(vs1.d3, sip.d3, vtemp.d3 );
}
/*---- get the p_{i}' = T_{i} p_{i} ----*/
errNum = Wlz3DSectionTransformVtxR(wlzViewStri, vs1.d3, &vtemp.d3 );
sip1p->d3.vtX = vtemp.d3.vtX;
sip1p->d3.vtY = vtemp.d3.vtY;
sip1p->d3.vtZ = vtemp.d3.vtZ;
printf("%lg %lg %lg\n", sip1p->d3.vtX, sip1p->d3.vtY, sip1p->d3.vtZ );
return errNum;
}
/*!
* - Function: WlzTrack3DVertical
* - Returns: WlzVetex
* - Purpose: Track vertex through neighbour layer.
* - Parameters:
*
* -# sip: input WlzVertex.
* -# sip1p: output WlzVertex by tracking.
* -# wlzViewStri: WlzThreeDViewStruct for layer i;
* -# wlzViewStrip1: WlzThreeDViewStruct for the next layer i+-1;
* -# UpOrDown: > 0 Up; <= 0 down;
* - Author: J. Rao, R. Baldock
*/
static WlzErrorNum WlzTrack3DVertical( WlzVertex sip,
WlzVertex *sip1p,
WlzThreeDViewStruct *wlzViewStri,
WlzThreeDViewStruct *wlzViewStrip1,
int UpOrDown)
{
WlzVertex vtemp, vtemp0, vs, vo, vso, vtx1;
WlzVertex vs1, nI, nIP1;
double dtemp, dtemp1, dtemp2;
WlzErrorNum errNum = WLZ_ERR_NONE;
/* output for test */
/*
printf("Yaw: %f\n", wlzViewStri->theta*180.0/3.14);
printf("Pitch: %f\n", wlzViewStri->phi*180.0/3.14 );
printf("Roll: %f\n", wlzViewStri->zeta*180.0/3.14 );
printf("distance: %f\n", wlzViewStri->dist );
printf("scaling: %f\n", wlzViewStri->scale );
printf("%f %f %f\n", wlzViewStri->fixed.vtX, wlzViewStri->fixed.vtY ,wlzViewStri->fixed.vtZ );
*/
/* vtemp0.d3 = (sx', sy', sz') */
WlzValueCopyDVertexToDVertex3(&vtemp0.d3, &sip.d3, 1);
vtemp0.d3.vtZ = wlzViewStri->dist;
/* reverse affine transformation the vertx to get vs.d3 = s_i here is in OPT space */
errNum = Wlz3DSectionTransformInvVtxR(wlzViewStri, vtemp0.d3, &vs.d3 );
printf("%f %f %f\n", vs.d3.vtX, vs.d3.vtY, vs.d3.vtZ);
/* as the vs.d3 is in OPT space and we want vertical tracking
we have:
vs1.d3.vtX = vs.d3.vtX
vs1.d3.vtY = vs.d3.vtY
vs1.d3.vtZ = ? need to be find !!!!!
vs1.d3.vtZ = vs.d3.vtZ + s
where s n_z dot n_{i+1} = [ O_{i+1} - r_{i} ] dot n_{i+1}
*/
/*
nz.d3.vtX = 0.0;
nz.d3.vtY = 0.0;
nz.d3.vtZ = 1.0;
if( UpOrDown <= 0 )
{
nz.d3.vtZ = -1.0;
}
*/
vtx1.d3.vtX = 0.0;
vtx1.d3.vtY = 0.0;
vtx1.d3.vtZ = 1.0;
/* get the normal n_i = nI.d3 of this plan i -plane do not need !!! */
errNum = Wlz3DSectionTransformInvVtxR(wlzViewStri, vtx1.d3, &nI.d3 );
/* get the normal n_i(+-1) = nIP1.d3 of the next plan i+-1 -plane */
errNum = Wlz3DSectionTransformInvVtxR(wlzViewStrip1, vtx1.d3, &nIP1.d3 );
/* track the s_{i+-1} vertically in OPT space */
/* check the n_{i+1}_z != 0 other wise throw an erro */
dtemp = nIP1.d3.vtZ;
/* dtemp = abs(dtemp); */
if( ( dtemp < 0.00000001 ) && ( dtemp > -0.00000001 ) )
{
printf("%lg\n",dtemp);
printf(" the z-component of n_{i+1} is zero in OPT space, we can't track vertically\n ");
printf("%lg %lg %lg\n", nIP1.d3.vtX, nIP1.d3.vtY, nIP1.d3.vtZ);
exit( 1 );
}
else
{
/* not parallel to z-direction */
/* s_{i+1} = S_{i} + n_z * s
= S_{i} + n_z * < [ O_{i+1} - r_{i} ] dot n_{i+1} / ( n_z dot n_{i+1} ) >
*/
/* dtemp = ( n_z dot n_{i+1} ) */
dtemp = WLZ_VTX_3_DOT(vtx1.d3, nIP1.d3);
/* get the o_i+1
vo.d3.vtX = wlzViewStr1->fixed.vtX + wlzViewStr->dist * nIP1.d3.vtX;
vo.d3.vtY = wlzViewStr1->fixed.vtY + wlzViewStr->dist * nIP1.d3.vtY;
vo.d3.vtY = wlzViewStr1->fixed.vtZ + wlzViewStr->dist * nIP1.d3.vtZ;
*/
WLZ_VTX_3_SCALE(vtemp0.d3, nIP1.d3, wlzViewStrip1->dist);
WLZ_VTX_3_ADD(vo.d3, wlzViewStrip1->fixed, vtemp0.d3);
/* vso.d3 = vo.d3 - vs.d3 */
WLZ_VTX_3_SUB(vso.d3,vo.d3,vs.d3);
/* vs0.d3 dot n_i+1 */
dtemp1 = WLZ_VTX_3_DOT(vso.d3, nIP1.d3);
/* get the s */
dtemp2 = dtemp1 / dtemp;
/* vtemp.d3 = ( n_z * s ) */
WLZ_VTX_3_SCALE(vtemp.d3, vtx1.d3, dtemp2 );
/* now get the s_{i+1} */
WLZ_VTX_3_ADD(vs1.d3, vs.d3, vtemp.d3 );
}
/*---- get the s_{i+1}' = T_{i+1} s_{i+1} ----*/
errNum = Wlz3DSectionTransformVtxR(wlzViewStrip1, vs1.d3, &vtemp.d3 );
sip1p->d3.vtX = vtemp.d3.vtX;
sip1p->d3.vtY = vtemp.d3.vtY;
sip1p->d3.vtZ = vtemp.d3.vtZ;
printf("%lg %lg %lg\n", sip1p->d3.vtX, sip1p->d3.vtY, sip1p->d3.vtZ );
return errNum;
}
