// matrix transform from source to destination
Matrix3x3
matTransformCanonical(double translateX,
double translateY,
double scaleX,
double scaleY,
double skewX,
double skewY,
bool skewOrderYX,
double rads,
double centerX,
double centerY)
{
///1) We translate to the center of the transform.
///2) We scale
///3) We apply skewX and skewY in the right order
///4) We rotate
///5) We apply the global translation
///5) We translate back to the origin
return matMul( matMul( matMul( matMul( matMul( matTranslation(centerX, centerY),
matTranslation(translateX, translateY) ),
matRotation(-rads) ),
matSkewXY(skewX, skewY, skewOrderYX) ),
matScale(scaleX, scaleY) ),
matTranslation(-centerX, -centerY) );
}
FeaturePoint* searchMahaNearestFeatPt(FeaturePoints& featPts, int f,
double m[2], double var[4], double maxDist) {
FeaturePoint* pHead = featPts.getFrameHead(f);
if (!pHead)
return 0;
double ivar[4];
mat22Inv(var, ivar);
matScale(2, 2, ivar, 1 / maxDist, ivar);
if (!featPts.getFrameTail(f))
return 0;
FeaturePoint* pEnd = featPts.getFrameTail(f)->next;
FeaturePoint* p = pHead;
double dMin = DBL_MAX;
FeaturePoint* pMin = 0;
while (p != pEnd) {
double d = mahaDist2(m, p->m, ivar);
if (d < dMin) {
dMin = d;
pMin = p;
}
p = p->next;
}
return pMin;
}
// matrix transform from destination to source
Matrix3x3
matInverseTransformCanonical(double translateX,
double translateY,
double scaleX,
double scaleY,
double skewX,
double skewY,
bool skewOrderYX,
double rads,
double centerX,
double centerY)
{
///1) We translate to the center of the transform.
///2) We scale
///3) We apply skewX and skewY in the right order
///4) We rotate
///5) We apply the global translation
///5) We translate back to the origin
// since this is the inverse, oerations are in reverse order
return matMul( matMul( matMul( matMul( matMul( matTranslation(centerX, centerY),
matScale(1. / scaleX, 1. / scaleY) ),
matSkewXY(-skewX, -skewY, !skewOrderYX) ),
matRotation(rads) ),
matTranslation(-translateX, -translateY) ),
matTranslation(-centerX, -centerY) );
}
static
Matrix3x3
matScaleAroundPoint(double scaleX,
double scaleY,
double px,
double py)
{
return matMul( matTranslation(px, py), matMul( matScale(scaleX, scaleY), matTranslation(-px, -py) ) );
}
/// transform from canonical coordinates to pixel coordinates
Matrix3x3
matCanonicalToPixel(double pixelaspectratio, //!< 1.067 for PAL, where 720x576 pixels occupy 768x576 in canonical coords
double renderscaleX, //!< 0.5 for a half-resolution image
double renderscaleY,
bool fielded) //!< true if the image property kOfxImagePropField is kOfxImageFieldLower or kOfxImageFieldUpper (apply 0.5 field scale in Y
{
/*
To map an X and Y coordinates from Canonical coordinates to Pixel coordinates, we perform the following multiplications...
X' = (X * SX)/PAR
Y' = Y * SY * FS
*/
// FIXME: when it's the Upper field, showuldn't the first pixel start at canonical coordinate (0,0.5) ?
return matScale( renderscaleX / pixelaspectratio, renderscaleY * (fielded ? 0.5 : 1.0) );
}
void S3D_MASTER::Render( bool aIsRenderingJustNonTransparentObjects,
bool aIsRenderingJustTransparentObjects )
{
if( m_parser == NULL )
return;
double aVrmlunits_to_3Dunits = g_Parm_3D_Visu.m_BiuTo3Dunits * UNITS3D_TO_UNITSPCB;
glScalef( aVrmlunits_to_3Dunits, aVrmlunits_to_3Dunits, aVrmlunits_to_3Dunits );
glm::vec3 matScale( m_MatScale.x,
m_MatScale.y,
m_MatScale.z );
glm::vec3 matRot( m_MatRotation.x,
m_MatRotation.y,
m_MatRotation.z );
glm::vec3 matPos( m_MatPosition.x,
m_MatPosition.y,
m_MatPosition.z );
glTranslatef( matPos.x * SCALE_3D_CONV,
matPos.y * SCALE_3D_CONV,
matPos.z * SCALE_3D_CONV );
glRotatef( -matRot.z, 0.0f, 0.0f, 1.0f );
glRotatef( -matRot.y, 0.0f, 1.0f, 0.0f );
glRotatef( -matRot.x, 1.0f, 0.0f, 0.0f );
glScalef( matScale.x, matScale.y, matScale.z );
for( unsigned int idx = 0; idx < m_parser->childs.size(); idx++ )
{
m_parser->childs[idx]->openGL_RenderAllChilds( aIsRenderingJustNonTransparentObjects,
aIsRenderingJustTransparentObjects );
}
}
/*
* Returns the *transposed* Jacobian matrix of the matrix cross product
* r12 x r23 = (r2 - r1) x (r3 - r2)
* where r1, r2 and r3 are vectors and differentiation is done with regards
* to ri (with i the given parameter) with all other rj (j != i) assumed to
* be fixed and independent of ri.
*
* The math is worked out in:
* http://www-personal.umich.edu/~riboch/pubfiles/riboch-JacobianofCrossProduct.pdf
* The result:
* J_ri(r12 x r23) = r12^x * J_ri(r23) - r23^x * J_ri(r12)
* where r12^x and r23^x are the matrix form of the cross product:
* ( 0 -Az Ay)
* A^x = ( Az 0 -Ax)
* (-Ay Ax 0 )
* and J_ri(r12) and J_ri(r23) are the jacobi matrices for r12 and r23,
* which are either 0, or +/- the identity matrix.
* We have:
* J_r1(r12) = -1, J_r2(r12) = +1, J_r3(r12) = 0,
* J_r1(r23) = 0, J_r2(r23) = -1, J_r3(r23) = +1.
*/
static __inline__ Mat3 crossProdTransposedJacobian(Vec3 r12, Vec3 r23, int i)
{
/* *Transposed* matrix form of the cross product. */
Mat3 r12matrCrossProd = mat3( 0, r12.z, -r12.y,
-r12.z, 0, r12.x,
r12.y, -r12.x, 0 );
/* *Transposed* matrix form of the cross product. */
Mat3 r23matrCrossProd = mat3( 0, r23.z, -r23.y,
-r23.z, 0, r23.x,
r23.y, -r23.x, 0 );
switch (i) {
case 1:
return r23matrCrossProd;
case 2:
return matScale(matAdd(r12matrCrossProd, r23matrCrossProd), -1);
case 3:
return r12matrCrossProd;
default:
die("Internal error!\n");
return mat3(0,0,0,0,0,0,0,0,0); /* To make compiler happy. */
}
}
void kfUpdate(float *xl, float *omega) {
static float phi = 0;
static float theta = 0;
static float psi = 0;
float dphi, dtheta, dP;
float sin_theta;
float cos_phi = cos(phi);
float sin_phi = sin(phi);
float cos_theta = cos(theta);
float tan_theta = tan(theta);
float a_values[] = { -wy*cos_phi*tan_theta + wz*sin_phi*tan_theta,
(-wy*sin_phi + wz*cos_phi) / (cos_theta*cos_theta),
wy*sin_phi - wz*cos_phi,
0 };
float c_values[6];
float h_values[3];
matAssignValues(A, a_values);
dphi = wx - wy*sin_phi*tan_theta - wz*cos_phi*tan_theta;
dtheta = -wy*cos_phi - wz*sin_phi;
phi = phi + dphi * TIME_STEP;
theta = theta + dtheta * TIME_STEP;
// computing dP = APA' + Q
matTranspose(temp2x2, A); // A'
matDotProduct(temp2x2, P, temp2x2); // PA'
matDotProduct(temp2x2, A, temp2x2); // APA'
matAdd(dP, temp2x2, Q); // APA' + Q
// computing P = P + dt*dP
matScale(temp2x2, dP, TIME_STEP); // dt*dP
matAdd(P, P, temp2x2); // P + dt*dP
cos_phi = cos(phi);
sin_phi = sin(phi);
cos_theta = cos(theta);
sin_theta = sin(theta);
c_values = {0, cos_theta,
-cos_theta*cos_phi, sin_theta*sin_phi,
cos_theta*sin_phi, sin_theta*cos_phi }
matAssignValues(C, c_values);
// L = PC'(R + CPC')^-1
matTranspose(temp2x3, C); // C'
matDotProduct(temp2x3, P, temp2x3); // PC'
matDotProduct(temp3x3, C, temp2x3); // CPC'
matAdd(temp3x3, R, temp3x3); // R + CPC'
matInverse(temp3x3, temp3x3); // (R + CPC')^-1
matDotProduct(L, temp2x3, temp3x3); // PC'(R + CPC')^-1
// P = (I - LC)P
matDotProduct(temp2x2, L, C); // LC
matSub(temp2x2, I, temp2x2); // I - LC
matDotProduct(P, temp2x2, P); // (I - LC)P
h_values = {sin_theta, -cos_theta*sin_phi, -cos_theta*cos_phi};
matAssignValues(H, h_values);
/*
ph = ph + dot(L[0], self.ab - h)
th = th + dot(L[1], self.ab - h)
*/
// change the values so that they stay between -PI and +PI
phi = ((phi + PI) % (2*PI)) - PI;
theta = ((theta + PI) % (2*PI)) - PI;
psidot = wy * sin(ph) / cos(th) + * cos(ph) / cos(th);
psi += psidot * TIME_STEP;
}
static
Matrix3x3
matScale(double s)
{
return matScale(s, s);
}
