/*
** The GSE to GSEQ transformation is given by the matrix
**
** T6 = <theta, X>
**
** where theta is the angle between the Y-axes in the two systems. A full
** description can be found at
** http://www-ssc.igpp.ucla.edu/personnel/russell/papers/gct1.html/#s3.5
** (Geophysical Coordinate Transformations, C. T. Russell 1971)
*/
void
mat_T6(const double et, Mat mat)
{
Vec GSE_ES, GEI_ES, thetaD;
double theta, thetaN, magThetaD;
Mat matT2;
/* Get Earth-Sun vector in GEI */
GSE_ES[0] = 1.0;
GSE_ES[1] = 0.0;
GSE_ES[2] = 0.0;
/* Convert GSE --> GEI */
mat_T2(et, matT2);
mat_transpose(matT2, matT2);
mat_times_vec(matT2, GSE_ES, GEI_ES);
/* Rotation axis of the Sun (GEI): (1.217,- 0.424, 0.897) */
thetaN = GEI_ES[0]*(-0.032) + GEI_ES[1]*(-0.112) + GEI_ES[2]*(-0.048);
thetaD[0] = (-0.424)*GEI_ES[2] - 0.897*GEI_ES[1];
thetaD[1] = 0.897*GEI_ES[0] - 0.1217*GEI_ES[2];
thetaD[2] = 0.1217*GEI_ES[1] - (-0.424)*GEI_ES[0];
magThetaD = sqrt(pow(thetaD[0],2) + pow(thetaD[1], 2) + pow(thetaD[2], 2));
theta = asin(thetaN/magThetaD);
/* printf("Theta: %f\n", theta); */
hapgood_matrix((theta*RADIANS_TO_DEGREES), X, mat);
/* TODO: Unknown why transpose is necessary to match previous results */
mat_transpose(mat,mat);
}
void CRenderLib::m_SetToonFinalPass(CMaterial *m)
{
AD_Matrix M1, M2;
HRESULT hr;
// risetto a wrap l'addressU e a linear i filtri
hr=p_Device->SetTextureStageState(0, D3DTSS_ADDRESSU, D3DTADDRESS_WRAP);
hr=p_Device->SetTextureStageState(0, D3DTSS_MAGFILTER, D3DTEXF_LINEAR);
hr=p_Device->SetTextureStageState(0, D3DTSS_MINFILTER, D3DTEXF_LINEAR);
if ((!m->p_Map1) || (m->p_Map1Channel!=TEXMAPPING1)) return;
mat_transpose(&m->p_Map1Matrix, &M1);
hr=p_Device->SetVertexShaderConstant(CV_MAP1UVMATRIX_0, (void *)&M1, 4);
mat_transpose(&m->p_Map2Matrix, &M2);
hr=p_Device->SetVertexShaderConstant(CV_MAP2UVMATRIX_0, (void *)&M2, 4);
hr=p_Device->SetRenderState(D3DRS_ALPHABLENDENABLE, TRUE);
hr=p_Device->SetRenderState(D3DRS_ZWRITEENABLE, TRUE);
switch (m->p_ToonMapsMixType)
{
case MIXTYPE_ADD:
hr=p_Device->SetRenderState(D3DRS_SRCBLEND, D3DBLEND_ONE);
hr=p_Device->SetRenderState(D3DRS_DESTBLEND, D3DBLEND_ONE);
break;
case MIXTYPE_ADDSMOOTH:
hr=p_Device->SetRenderState(D3DRS_SRCBLEND, D3DBLEND_SRCCOLOR);
hr=p_Device->SetRenderState(D3DRS_DESTBLEND, D3DBLEND_ONE);
break;
case MIXTYPE_MODULATE2X:
hr=p_Device->SetRenderState(D3DRS_SRCBLEND, D3DBLEND_DESTCOLOR);
hr=p_Device->SetRenderState(D3DRS_DESTBLEND, D3DBLEND_SRCCOLOR);
break;
default:
hr=p_Device->SetRenderState(D3DRS_SRCBLEND, D3DBLEND_DESTCOLOR);
hr=p_Device->SetRenderState(D3DRS_DESTBLEND, D3DBLEND_ZERO);
break;
}
hr=p_Device->SetTexture(0, m->p_Map1->p_HWSurfaces[m->p_Map1->p_CurrentTextureIndex]);
hr=p_Device->SetTextureStageState(0, D3DTSS_COLORARG1, D3DTA_TEXTURE);
hr=p_Device->SetTextureStageState(0, D3DTSS_COLOROP, D3DTOP_SELECTARG1);
hr=p_Device->SetTextureStageState(0, D3DTSS_TEXCOORDINDEX, 0);
hr=p_Device->SetTexture(1, NULL);
hr=p_Device->SetTextureStageState(1, D3DTSS_COLOROP, D3DTOP_DISABLE);
hr=p_Device->SetTexture(2, NULL);
hr=p_Device->SetTextureStageState(2, D3DTSS_COLOROP, D3DTOP_DISABLE);
hr=p_Device->SetVertexShader(p_TOONFinalPassHandle);
}
static enum efp_result
set_coord_points(struct frag *frag, const double *coord)
{
if (frag->n_atoms < 3) {
efp_log("fragment must contain at least three atoms");
return EFP_RESULT_FATAL;
}
double ref[9] = {
frag->lib->atoms[0].x, frag->lib->atoms[0].y, frag->lib->atoms[0].z,
frag->lib->atoms[1].x, frag->lib->atoms[1].y, frag->lib->atoms[1].z,
frag->lib->atoms[2].x, frag->lib->atoms[2].y, frag->lib->atoms[2].z
};
vec_t p1;
mat_t rot1, rot2;
efp_points_to_matrix(coord, &rot1);
efp_points_to_matrix(ref, &rot2);
rot2 = mat_transpose(&rot2);
frag->rotmat = mat_mat(&rot1, &rot2);
p1 = mat_vec(&frag->rotmat, VEC(frag->lib->atoms[0].x));
/* center of mass */
frag->x = coord[0] - p1.x;
frag->y = coord[1] - p1.y;
frag->z = coord[2] - p1.z;
update_fragment(frag);
return EFP_RESULT_SUCCESS;
}
void palRevoluteLink::GetPosition(palVector3& pos) const {
//Convert link_rel to the global coordinate system
//Link_abs=(Link_rel * R_Inv) - parent_abs
//Transpose the matrix to get Normal rotation matrixes.
palMatrix4x4 a_PAL = m_pParent->GetLocationMatrix();
palMatrix4x4 a; //R
mat_transpose(&a, &a_PAL);
palMatrix4x4 a_inv; //R_Inv
palVector3 link_rel;
palVector3 link_abs;
link_rel.x =m_fRelativePosX;
link_rel.y =m_fRelativePosY;
link_rel.z =m_fRelativePosZ;
bool isInverted = mat_invert(&a_inv,&a);
if(!isInverted)
return;
vec_mat_mul(&link_abs,&a_inv,&link_rel);
palVector3 posVec;
m_pParent->GetPosition(posVec);
pos.x = link_abs.x + posVec.x;
pos.y = link_abs.y + posVec.y;
pos.z = link_abs.z + posVec.z;
}
/*
** vec_Qe
**
** don't ask.
*/
void
vec_Qe(double et, Vec Qe)
{
double lat = mag_lat(et);
double lon = mag_lon(et);
double cos_lat = cos(lat);
double sin_lat = sin(lat);
double cos_lon = cos(lon);
double sin_lon = sin(lon);
Mat mat_tmp, mat;
Vec Qg;
Qg[0] = cos_lat * cos_lon;
Qg[1] = cos_lat * sin_lon;
Qg[2] = sin_lat;
/* printf("lat=%lf lon=%lf\n", 90.0 - lat, lon);*/
mat_T2(et, mat);
mat_T1(et, mat_tmp);
mat_transpose(mat_tmp, mat_tmp);
mat_times_mat(mat, mat_tmp, mat);
mat_times_vec(mat, Qg, Qe);
}
void ekf_filter_predict(struct ekf_filter* filter, double *u) {
/* prediction :
X += Xdot * dt
Pdot = F * P * F' + Q ( or Pdot = F*P + P*F' + Q for continuous form )
P += Pdot * dt
*/
int n = filter->state_dim;
double dt;
/* fetch dt, Xdot and F */
filter->ffun(u, filter->X, &dt, filter->Xdot, filter->F);
/* X = X + Xdot * dt */
mat_add_scal_mult(n, 1, filter->X, filter->X, dt, filter->Xdot);
#ifdef EKF_UPDATE_CONTINUOUS
/*
continuous update
Pdot = F * P + P * F' + Q
*/
mat_mult(n, n, n, filter->tmp1, filter->F, filter->P);
mat_transpose(n, n, filter->tmp2, filter->F);
mat_mult(n, n, n, filter->tmp3, filter->P, filter->tmp2);
mat_add(n, n, filter->Pdot, filter->tmp1, filter->tmp3);
mat_add(n, n, filter->Pdot, filter->Pdot, filter->Q);
#endif
#ifdef EKF_UPDATE_DISCRETE
/*
discrete update
Pdot = F * P * F' + Q
*/
mat_mult(n, n, n, filter->tmp1, filter->F, filter->P);
mat_transpose(n, n, filter->tmp2, filter->F);
mat_mult(n, n, n, filter->tmp3, filter->tmp1, filter->tmp2);
mat_add(n, n, filter->Pdot, filter->tmp3, filter->Q);
#endif
/* P = P + Pdot * dt */
mat_add_scal_mult(n, n, filter->P, filter->P, dt, filter->Pdot);
}
void AD_TaperModifier::update(float4 framepos)
{
AD_Vect3D sub_center;
AD_Matrix mrot, imrot, pret, ipret;
if (center_track!=(postrack *)NULL)
center_track->get_data(framepos, ¢er);
if (amount_track!=(rolltrack *)NULL)
amount_track->get_data(framepos, &amount);
if (curve_track!=(rolltrack *)NULL)
curve_track->get_data(framepos, &curve);
if (uplim_track!=(rolltrack *)NULL)
uplim_track->get_data(framepos, &uplim);
if (lowlim_track!=(rolltrack *)NULL)
lowlim_track->get_data(framepos, &lowlim);
// costruzione matrice di trasformazione e la sua
// inversa
sub_center.x=-center.x;
sub_center.y=-center.y;
sub_center.z=-center.z;
switch (axis)
{
case 0: // asse X
mat_setmatrix_of_eulerrotationZ(&mrot, (float)(-M_PI/2.0));
l=bbx2-bbx1;
break;
case 1: // asse Y
mat_identity(&mrot);
l=bby2-bby1;
break;
case 2: // asse Z
mat_setmatrix_of_eulerrotationX(&mrot, (float)(M_PI/2.0));
l=bbz2-bbz1;
break;
}
mat_transpose(&mrot, &imrot);
mat_setmatrix_of_pretraslation(&pret, &sub_center);
mat_setmatrix_of_pretraslation(&ipret, ¢er);
mat_mul (&mrot, &pret, &tm);
mat_mul (&ipret, &imrot, &invtm);
switch (effectaxis)
{
case 0: doX = 1; doY = 0; break;
case 1: doX = 0; doY = 1; break;
case 2: doX = 1; doY = 1; break;
}
}
void AD_TwistModifier::update(float4 framepos)
{
AD_Vect3D sub_center;
AD_Matrix mrot, imrot, pret, ipret;
if (center_track!=(postrack *)NULL)
center_track->get_data(framepos, ¢er);
if (angle_track!=(rolltrack *)NULL)
angle_track->get_data(framepos, &angle);
if (bias_track!=(rolltrack *)NULL)
bias_track->get_data(framepos, &bias);
if (uplim_track!=(rolltrack *)NULL)
uplim_track->get_data(framepos, &uplim);
if (lowlim_track!=(rolltrack *)NULL)
lowlim_track->get_data(framepos, &lowlim);
// costruzione matrice di trasformazione e la sua
// inversa
sub_center.x=-center.x;
sub_center.y=-center.y;
sub_center.z=-center.z;
switch (axis)
{
case 0: // asse X
mat_setmatrix_of_eulerrotationZ(&mrot, (float)(-M_PI/2.0));
height=bbx2-bbx1;
break;
case 1: // asse Y
mat_identity(&mrot);
height=bby2-bby1;
break;
case 2: // asse Z
mat_setmatrix_of_eulerrotationX(&mrot, (float)(M_PI/2.0));
height=bbz2-bbz1;
break;
}
mat_transpose(&mrot, &imrot);
mat_setmatrix_of_pretraslation(&pret, &sub_center);
mat_setmatrix_of_pretraslation(&ipret, ¢er);
mat_mul (&mrot, &pret, &tm);
mat_mul (&ipret, &imrot, &invtm);
if (height==0)
{
angle = 0.0f;
angleOverHeight = 0.0f;
}
else angleOverHeight = angle / height;
}
void AD_WindModifier::build_objectmatrix (float4 framepos)
{
AD_Vect3D postmp, stmp;
AD_Quaternion objrot;
AD_Matrix posttrans, scaling, maux;
accum_scale.x=accum_scale.y=accum_scale.z=1.0f;
mat_identity(¤tmatrix_rot);
// estrazione dei dati col keyframer: niente di piu' facile col c++ !!!
if (rotationtrack.numkey>0)
{
rotationtrack.get_data(framepos, &objrot);
quat_rotquat_to_matrix(&objrot, ¤tmatrix_rot);
}
mat_copy(¤tmatrix_rot, ¤tmatrix);
if (scaletrack.numkey>0)
{
scaletrack.get_data(framepos, &stmp);
mat_setmatrix_of_scaling(&scaling, stmp.x, stmp.y, stmp.z);
accum_scale.x*=stmp.x;
accum_scale.y*=stmp.y;
accum_scale.z*=stmp.z;
}
else mat_identity(&scaling);
if (positiontrack.numkey>0) positiontrack.get_data(framepos, ¤tpos);
mat_setmatrix_of_pretraslation(&posttrans, ¤tpos);
mat_mul(&scaling, ¤tmatrix_rot, &maux);
mat_mul(&posttrans, &maux, ¤tmatrix);
if (father!=(AD_Object3D *)NULL)
{
mat_mulaffine(&father->currentmatrix_rot, ¤tmatrix_rot, ¤tmatrix_rot);
mat_mul(&father->currentmatrix, ¤tmatrix, ¤tmatrix);
mat_mulvect(&father->currentmatrix, ¤tpos, &postmp);
vect_copy(&postmp, ¤tpos);
accum_scale.x*=father->accum_scale.x;
accum_scale.y*=father->accum_scale.y;
accum_scale.z*=father->accum_scale.z;
}
mat_transpose(¤tmatrix_rot, &inverse_rotmatrix);
mat_get_row(¤tmatrix, 1, &forza);
vect_auto_normalize(&forza);
vect_scale(&forza, strenght*0.00016f*forceScaleFactor, &forza);
}
void AD_PatchObject::build_objectmatrix (float4 framepos)
// costruisce la matrice di trasformazione, che servira' poi per trasformare
// i vertici dell'oggetto;
{
AD_Vect3D postmp, stmp;
AD_Quaternion objrot;
AD_Matrix posttrans, scaling, maux;
accum_scale.x=accum_scale.y=accum_scale.z=1.0f;
mat_identity(¤tmatrix_rot);
// estrazione dei dati col keyframer: niente di piu' facile col c++ !!!
if (rotationtrack.numkey>0)
{
rotationtrack.get_data(framepos, &objrot);
quat_rotquat_to_matrix(&objrot, ¤tmatrix_rot);
}
mat_copy(¤tmatrix_rot, ¤tmatrix);
if (scaletrack.numkey>0)
{
scaletrack.get_data(framepos, &stmp);
mat_setmatrix_of_scaling(&scaling, stmp.x, stmp.y, stmp.z);
accum_scale.x=accum_scale.x*stmp.x;
accum_scale.y=accum_scale.x*stmp.y;
accum_scale.z=accum_scale.x*stmp.z;
}
else mat_identity(&scaling);
if (positiontrack.numkey>0) positiontrack.get_data(framepos, ¤tpos);
mat_setmatrix_of_pretraslation(&posttrans, ¤tpos);
mat_mul(&scaling, ¤tmatrix_rot, &maux);
mat_mul(&posttrans, &maux, ¤tmatrix);
if (father!=(AD_Object3D *)NULL)
{
mat_mulaffine(&father->currentmatrix_rot, ¤tmatrix_rot, ¤tmatrix_rot);
mat_mul(&father->currentmatrix, ¤tmatrix, ¤tmatrix);
mat_mulvect(&father->currentmatrix, ¤tpos, &postmp);
vect_copy(&postmp, ¤tpos);
accum_scale.x*=father->accum_scale.x;
accum_scale.y*=father->accum_scale.y;
accum_scale.z*=father->accum_scale.z;
}
mat_transpose(¤tmatrix_rot, &inverse_rotmatrix);
}
void ekf_filter_update(struct ekf_filter* filter, double *y) {
/* update
E = H * P * H' + R
K = P * H' * inv(E)
P = P - K * H * P
X = X + K * err
*/
double err;
int n = filter->state_dim;
int m = filter->measure_dim;
filter->mfun(y, &err, filter->X, filter->H);
/* E = H * P * H' + R */
mat_mult(m, n, n, filter->tmp1, filter->H, filter->P);
mat_transpose(m, n, filter->tmp2, filter->H);
mat_mult(m, n, m, filter->tmp3, filter->tmp1, filter->tmp2);
mat_add(m, m, filter->E, filter->tmp3, filter->R);
/* K = P * H' * inv(E) */
mat_transpose(m, n, filter->tmp1, filter->H);
mat_mult(n, n, m, filter->tmp2, filter->P, filter->tmp1);
if (filter->measure_dim != 1) { printf("only dim 1 measure implemented for now\n"); exit(-1);}
mat_scal_mult(n, m, filter->K, 1./filter->E[0], filter->tmp2);
/* P = P - K * H * P */
mat_mult(n, m, n, filter->tmp1, filter->K, filter->H);
mat_mult(n, n, n, filter->tmp2, filter->tmp1, filter->P);
mat_sub(n, n, filter->P, filter->P, filter->tmp2);
/* X = X + err * K */
mat_add_scal_mult(n, m, filter->X, filter->X, err, filter->K);
}
void CGeometricObject::m_BuildWorldMatrix(void)
{
AD_Vect3D postmp;
AD_Matrix posttrans, scaling, maux;
AD_Matrix iRot, iScale, iTrans;
// matrice di rotazione e sua inversa
quat_rotquat_to_matrix(&p_CurrentRotationQuaternion, &p_CurrentRotationMatrix);
mat_copy(&p_CurrentRotationMatrix, &p_WorldMatrix);
mat_transpose(&p_CurrentRotationMatrix, &iRot);
// matrice di scaling e sua inversa
mat_setmatrix_of_scaling(&scaling, p_CurrentScale.x, p_CurrentScale.y, p_CurrentScale.z);
mat_setmatrix_of_scaling(&iScale, 1.0f/p_CurrentScale.x, 1.0f/p_CurrentScale.y, 1.0f/p_CurrentScale.z);
// matrice di traslazione e sua inversa
mat_setmatrix_of_pretraslation(&posttrans, &p_CurrentPosition);
vect_neg(&p_CurrentPosition, &postmp);
mat_setmatrix_of_pretraslation(&iTrans, &postmp);
// prima pivot, poi scaling, poi rotazione, e infine traslazione
//vect_neg(&p_Pivot, &postmp);
//mat_setmatrix_of_pretraslation(&pivot, &postmp);
//mat_mul(&scaling, &pivot, &maux);
//mat_mul(&p_CurrentRotationMatrix, &maux, &maux);
mat_mul(&p_CurrentRotationMatrix, &scaling, &maux);
mat_mul(&posttrans, &maux, &p_WorldMatrix);
// per la inversa: prima traslazione, rotazione, scaling e pivot
//mat_setmatrix_of_pretraslation(&iPivot, &p_Pivot);
mat_mul(&iRot, &iTrans, &maux);
mat_mul(&iScale, &maux, &p_InverseWorldMatrix);
//mat_mul(&iScale, &maux, &maux);
//mat_mul(&iPivot, &maux, &p_InverseWorldMatrix);
if (p_Father)
{
mat_mulaffine(&p_Father->p_CurrentRotationMatrix,
&p_CurrentRotationMatrix,
&p_CurrentRotationMatrix);
mat_mul(&p_Father->p_WorldMatrix, &p_WorldMatrix, &p_WorldMatrix);
mat_mul(&p_InverseWorldMatrix, &p_Father->p_InverseWorldMatrix, &p_InverseWorldMatrix);
p_TotalScale.x*=p_Father->p_TotalScale.x;
p_TotalScale.y*=p_Father->p_TotalScale.y;
p_TotalScale.z*=p_Father->p_TotalScale.z;
}
}
void CCamera::m_BuildWorldMatrix(void)
{
float4 sinx, cosx, siny, cosy, sinz, cosz;
AD_Matrix pretrans, irot, swapXY;
AD_Vect3D ptmp;
sinx = fast_sinf(p_CurrentAngX);
cosx = fast_cosf(p_CurrentAngX);
siny = fast_sinf(p_CurrentAngY);
cosy = fast_cosf(p_CurrentAngY);
sinz = fast_sinf(p_CurrentAngZ);
cosz = fast_cosf(p_CurrentAngZ);
vect_neg(&p_CurrentPosition, &ptmp);
mat_setmatrix_of_pretraslation(&pretrans, &ptmp);
mat_identity(&p_CurrentRotationMatrix);
p_CurrentRotationMatrix.a[0][0] = sinx * siny * sinz + cosx * cosz;
p_CurrentRotationMatrix.a[0][1] = cosy * sinz;
p_CurrentRotationMatrix.a[0][2] = sinx * cosz - cosx * siny * sinz;
p_CurrentRotationMatrix.a[1][0] = sinx * siny * cosz - cosx * sinz;
p_CurrentRotationMatrix.a[1][1] = cosy * cosz;
p_CurrentRotationMatrix.a[1][2] = -cosx * siny * cosz - sinx * sinz;
p_CurrentRotationMatrix.a[2][0] = -sinx * cosy;
p_CurrentRotationMatrix.a[2][1] = siny;
p_CurrentRotationMatrix.a[2][2] = cosx * cosy;
mat_mul(&p_CurrentRotationMatrix, &pretrans, &p_ViewMatrix);
// costruzione matrice inversa
mat_transpose(&p_CurrentRotationMatrix, &irot);
mat_setmatrix_of_pretraslation(&pretrans, &p_CurrentPosition);
mat_mul(&pretrans, &irot, &p_InverseViewMatrix);
// costruzione matrice per il linking con oggetti
// 1) matrice di swap YZ
mat_identity(&swapXY);
swapXY.a[1][1]=0;
swapXY.a[2][2]=0;
swapXY.a[1][2]=1;
swapXY.a[2][1]=-1;
mat_mul(&p_InverseViewMatrix, &swapXY, &p_WorldMatrix);
//mat_mul(&swapXY, &p_InverseViewMatrix, &p_WorldMatrix);
//mat_copy(&p_InverseViewMatrix, &p_WorldMatrix);
}
// [RING] Calculate Pseudoinverse
static void calc_pinv(unsigned int Nint, complex float pinv[3][2 * Nint], const complex float A[2 * Nint][3])
{
//AH
complex float AH[3][2 * Nint];
mat_transpose(2 * Nint, 3, AH, A);
// (AH * A)^-1
complex float dot[3][3];
mat_mul(3, 2 * Nint, 3, dot, AH, A);
complex float inv[3][3];
mat_inverse(3, inv, dot);
// (AH * A)^-1 * AH
mat_mul(3, 3, 2 * Nint, pinv, inv, AH);
return;
}
static enum efp_result
set_coord_points(struct frag *frag, const double *coord)
{
/* allow fragments with less than 3 atoms by using multipole points of
* ghost atoms; multipole points have the same coordinates as atoms */
if (frag->n_multipole_pts < 3) {
efp_log("fragment must contain at least three atoms");
return EFP_RESULT_FATAL;
}
double ref[9] = {
frag->lib->multipole_pts[0].x,
frag->lib->multipole_pts[0].y,
frag->lib->multipole_pts[0].z,
frag->lib->multipole_pts[1].x,
frag->lib->multipole_pts[1].y,
frag->lib->multipole_pts[1].z,
frag->lib->multipole_pts[2].x,
frag->lib->multipole_pts[2].y,
frag->lib->multipole_pts[2].z
};
vec_t p1;
mat_t rot1, rot2;
efp_points_to_matrix(coord, &rot1);
efp_points_to_matrix(ref, &rot2);
rot2 = mat_transpose(&rot2);
frag->rotmat = mat_mat(&rot1, &rot2);
p1 = mat_vec(&frag->rotmat, VEC(frag->lib->multipole_pts[0].x));
/* center of mass */
frag->x = coord[0] - p1.x;
frag->y = coord[1] - p1.y;
frag->z = coord[2] - p1.z;
update_fragment(frag);
return EFP_RESULT_SUCCESS;
}
/*********************\
** simple_rotation ** utility function used by all the "twixt" functions
**********************
**
** This is basically what all the "twixt" functions do:
**
** 1) define a rotation matrix
** 2) If doing an inverse transformation, transpose that matrix.
** 3) multiply the rotation matrix by the input vector.
**
** To save all that work in the "twixt" functions, they just call this
** function, passing us a pointer to a function that defines the matrix.
*/
int
simple_rotation(const double et, Vec v_in, Vec v_out, Direction d, void (*m)())
{
Mat mat;
/*
** Call the user-specified function to get a rotation matrix.
*/
(m)(et, mat);
/*
** To do the inverse transformation, we use the transposition of the matrix
*/
if (d == BACK)
mat_transpose(mat, mat);
/*
** Multiply the rotation matrix by the input vector, and that's it!
*/
mat_times_vec(mat, v_in, v_out);
return 0;
}
void palRevoluteLink::Init(palBodyBase *parent, palBodyBase *child, Float x, Float y, Float z, Float axis_x, Float axis_y, Float axis_z) {
palLink::Init(parent, child, x, y, z);
if (m_pParent && m_pChild) {
//Link_rel with the rotation matrix
//Link_rel=(link_abs - parent_abs)*R
palMatrix4x4 a_PAL = m_pParent->GetLocationMatrix();
palMatrix4x4 b_PAL = m_pChild->GetLocationMatrix();
// Transpose each body's position matrix to get its world-to-body
// rotation matrix:
palMatrix4x4 a, b;
mat_transpose(&a, &a_PAL); // a <- a_PAL'
mat_transpose(&b, &b_PAL); // b <- b_PAL'
palVector3 link_rel;
palVector3 translation;
// Compute the position of the link with respect to the parent's
// origin, in the parent's coordinate system:
palVector3 posVecParent;
m_pParent->GetPosition(posVecParent);
translation._vec[0] = m_fPosX - posVecParent.x;
translation._vec[1] = m_fPosY - posVecParent.y;
translation._vec[2] = m_fPosZ - posVecParent.z;
//Rotation
vec_mat_mul(&link_rel,&a,&translation);
m_fRelativePosX = link_rel.x;
m_fRelativePosY = link_rel.y;
m_fRelativePosZ = link_rel.z;
m_pivotA.x = m_fRelativePosX;
m_pivotA.y = m_fRelativePosY;
m_pivotA.z = m_fRelativePosZ;
// Compute the position of the link with respect to the child's
// origin, in the child's coordinate system:
palVector3 posVecChild;
m_pChild->GetPosition(posVecChild);
//link relative position with respect to the child
//Translation of absolute to relative
translation._vec[0] = m_fPosX - posVecChild.x; // first in world coords
translation._vec[1] = m_fPosY - posVecChild.y;
translation._vec[2] = m_fPosZ - posVecChild.z;
vec_mat_mul(&link_rel,&b,&translation); // rotate into child coords
m_pivotB.x = link_rel.x;
m_pivotB.y = link_rel.y;
m_pivotB.z = link_rel.z;
//Frames A and B: Bullet method
// Define a hinge coordinate system by generating hinge-to-body
// transforms for both parent and child bodies. The hinge
// coordinate system's +Z axis coincides with the hinge axis, its
// +X and +Y axes are perpendicular to the hinge axis, and its
// origin is at the hinge's origin.
palVector3 axis, m_axisA, m_axisB;
vec_set(&axis, axis_x, axis_y, axis_z);
vec_mat_mul(&m_axisA, &a, &axis); // axis in parent coords
m_fRelativeAxisX = m_axisA.x;
m_fRelativeAxisY = m_axisA.y;
m_fRelativeAxisZ = m_axisA.z;
vec_mat_mul(&m_axisB, &b, &axis); // axis in child coords
vec_norm(&m_axisB);
// Build m_frameA, which transforms points from hinge coordinates
// to parent (body A) coordinates.
// Choose basis vectors for the hinge coordinate system wrt parent coords:
palVector3 rbAxisA1( 1, 0, 0 ), rbAxisA2;
const palVector3 Z_AXIS( 0, 0, 1 );
// XXX debug
Float projection = vec_dot( & m_axisA, & Z_AXIS );
if (projection >= 1 - FLOAT_EPSILON) {
// The hinge axis coincides with the parent's +Z axis.
rbAxisA2 = palVector3( 0, 1, 0 );
} else if (projection <= -1 + FLOAT_EPSILON) {
// The hinge axis coincides with the parent's -Z axis.
rbAxisA2 = palVector3( 0, -1, 0 );
} else {
// The hinge axis crosses the parent's Z axis.
vec_cross( & rbAxisA2, & m_axisA, & Z_AXIS );
vec_cross( & rbAxisA1, & rbAxisA2, & m_axisA );
vec_norm( & rbAxisA1 );
vec_norm( & rbAxisA2 );
}
vec_norm( & m_axisA );
// Set frameA. Transform m_frameA maps points from hinge coordinate system,
// whose +Z axis coincides with the hinge axis, to the parent body's
// coordinate system.
mat_identity(&m_frameA);
mat_set_translation(&m_frameA,m_pivotA.x,m_pivotA.y,m_pivotA.z);
// Put the basis vectors in the columns of m_frameA:
m_frameA._11 = rbAxisA1.x;
m_frameA._12 = rbAxisA1.y;
m_frameA._13 = rbAxisA1.z;
m_frameA._21 = rbAxisA2.x;
m_frameA._22 = rbAxisA2.y;
m_frameA._23 = rbAxisA2.z;
m_frameA._31 = m_axisA.x;
m_frameA._32 = m_axisA.y;
m_frameA._33 = m_axisA.z;
palVector3 rbAxisB1, rbAxisB2;
if (true) {
//build frame B, see bullet for algo
palQuaternion rArc;
q_shortestArc(&rArc,&m_axisA,&m_axisB);
vec_q_rotate(&rbAxisB1,&rArc,&rbAxisA1);
vec_cross(&rbAxisB2,&m_axisB,&rbAxisB1);
} else {
palVector3 tmp;
vec_mat_mul( &tmp, &a_PAL, &rbAxisA1 );
vec_mat_mul( &rbAxisB1, &b, &tmp );
vec_mat_mul( &tmp, &a_PAL, &rbAxisA2 );
vec_mat_mul( &rbAxisB2, &b, &tmp );
}
// Build m_frameB, which transforms points from hinge coordinates
// to parent (body A) coordinates.
vec_norm(&rbAxisB1);
vec_norm(&rbAxisB2);
mat_identity(&m_frameB);
mat_set_translation(&m_frameB,m_pivotB.x,m_pivotB.y,m_pivotB.z);
// Put the basis vectors in the columns of m_frameB:
m_frameB._11 = rbAxisB1.x;
m_frameB._12 = rbAxisB1.y;
m_frameB._13 = rbAxisB1.z;
m_frameB._21 = rbAxisB2.x;
m_frameB._22 = rbAxisB2.y;
m_frameB._23 = rbAxisB2.z;
m_frameB._31 = m_axisB.x;
m_frameB._32 = m_axisB.y;
m_frameB._33 = m_axisB.z;
}
}
int AD_Object3D::init(void)
{
// AD_Lod lodder;
int k;
AD_Quaternion q;
AD_Matrix M;
//AD_Vect3D v;
float x1, y1, z1, x2, y2, z2;
AD_OSMObject *OSMaux[100];
// ############ INIZIALIZZAZIONI DELLE KEYFRAMER ############
if (positiontrack.init()==-1) return(-1);
if (rotationtrack.init()==-1) return(-1);
if (scaletrack.init()==-1) return(-1);
// cistruisco le quantita' che mi permettono di costruire una
// eventuale matrice di skin se questo oggetto fosse usato come
// osso
if (rotationtrack.numkey>0)
{
rotationtrack.get_data(0, &q);
quat_rotquat_to_matrix(&q, &M);
mat_transpose(&M, &rot0);
}else mat_identity(&rot0);
if (scaletrack.numkey>0) scaletrack.get_data(0, &scale0);
else vect_set(&scale0, 1, 1, 1);
if (positiontrack.numkey>0) positiontrack.get_data(0, &pos0);
else vect_set(&pos0, 0, 0, 0);
/*
if ((positiontrack.numkey==1) && (positiontrack.posizioni[0].posintrack==0) &&
(father==(AD_Object3D *)NULL) && (have_childrens<=0))
{
currentpos=positiontrack.posizioni[0].p;
positiontrack.numkey=0;
}
*/
if (type==DUMMY)
{
// sistemo il flare connesso; devo creare i 2 triangoli,
// i vertici 2D, 3D (solo per linkare i v2D), il materiale
// trasparente
if (flare!=(texture *)NULL)
{
latoX=(float)flare->dimx;
latoY=(float)flare->dimy;
// setto le uv dei vertici per il flare; i vertici sono cosi'
// disposti
// 0----1
// | |
// | |
// 2----3
vertex2D[0].u=0.0f*TEXTURE_PRECISION;
vertex2D[0].v=0.0f*TEXTURE_PRECISION;
vertex2D[1].u=0.99f*TEXTURE_PRECISION;
vertex2D[1].v=0.0f*TEXTURE_PRECISION;
vertex2D[2].u=0.0f*TEXTURE_PRECISION;
vertex2D[2].v=0.99f*TEXTURE_PRECISION;
vertex2D[3].u=0.99f*TEXTURE_PRECISION;
vertex2D[3].v=0.99f*TEXTURE_PRECISION;
// i vertici 3D li setto sempre da non clippare visto
// che quando devo disegnare i flare sono sicuro che non
// devo clipparli e solo quando disegno li inserisco nella
// lista dei triangoli trasparenti
vertex3D[0].flags=0;
vertex3D[1].flags=0;
vertex3D[2].flags=0;
vertex3D[3].flags=0;
// setto il materiale
matflare.texture_ptr=flare;
matflare.flags=(0 | PAINT_TEXTURE | IS_TRASPARENT);
matflare.trasparencytype=MIXTYPE_ADD;
AddUpdate_Material(&matflare);
// ATTENZIONE: non cambiare gli indici perche' a causa
// del settaggio di cull2D della scheda3D i triangoli
// devono essere in senso antiorario
// setto i triangoli
tria[0].materiale=&matflare;
tria[0].v1=&vertex3D[0];
tria[0].v2=&vertex3D[2];
tria[0].v3=&vertex3D[3];
tria[0].sp1=&vertex2D[0];
tria[0].sp2=&vertex2D[2];
tria[0].sp3=&vertex2D[3];
tria[1].materiale=&matflare;
tria[1].v1=&vertex3D[3];
tria[1].v2=&vertex3D[1];
tria[1].v3=&vertex3D[0];
tria[1].sp1=&vertex2D[3];
tria[1].sp2=&vertex2D[1];
tria[1].sp3=&vertex2D[0];
}
return(0);
}
if (type==BONE) return(0);
/* NON PIU' FATTIBILE A CAUSA DEGLI OSM
------------------------------------------
-----------------------------------------
// OTTIMIZAZIONE !!!
if ((rotationtrack.numkey==1) && (rotationtrack.rotazioni[0].posintrack==0) &&
(father==(AD_Object3D *)NULL) && (have_childrens<=0))
{
q=rotationtrack.rotazioni[0].rotquat;
quat_rotquat_to_matrix(&q, &M);
rotationtrack.numkey=0;
mat_identity(¤tmatrix_rot);
for (k=0; k<num_vertex; k++)
{
mat_mulvect(&M, &points[k], &v);
vect_copy(&v, &points[k]);
}
}
if ((scaletrack.numkey==1) && (scaletrack.posizioni[0].posintrack==0) &&
(father==(AD_Object3D *)NULL) && (have_childrens<=0))
{
for (k=0; k<num_vertex; k++)
{
points[k].x*=scaletrack.posizioni[0].p.x;
points[k].y*=scaletrack.posizioni[0].p.y;
points[k].z*=scaletrack.posizioni[0].p.z;
}
scaletrack.numkey=0;
}
*/
// calcolo bounding box per poi settarla negli OSM
x1=y1=z1=1E10;
x2=y2=z2=-1E10;
for (k=0; k<num_vertex3D; k++)
{
if (vertex3D[k].point.x < x1) x1 = vertex3D[k].point.x;
if (vertex3D[k].point.y < y1) y1 = vertex3D[k].point.y;
if (vertex3D[k].point.z < z1) z1 = vertex3D[k].point.z;
if (vertex3D[k].point.x > x2) x2 = vertex3D[k].point.x;
if (vertex3D[k].point.y > y2) y2 = vertex3D[k].point.y;
if (vertex3D[k].point.z > z2) z2 = vertex3D[k].point.z;
}
// inverto l'ordine degli OSM perche' vanno applicati
// al contrario
for (k=0; k<(int)num_OSMmods; k++)
OSMaux[k]=OSMmods[k];
for (k=0; k<(int)num_OSMmods; k++)
OSMmods[num_OSMmods-k-1]=OSMaux[k];
// Inizializzazione OSM modifiers
for (k=0; k<(int)num_OSMmods; k++)
{
OSMmods[k]->init();
OSMmods[k]->set_bbox(x1, y1, z1, x2, y2, z2);
}
precalc_radius(); // precalcola raggio sfera circoscritta
init_tria(); // calcola: normale, punto medio e raggio circoscritto
init_texture_coordinate(); // rende positive le uv
init_vertex(); // alloca e prepara le normali ai vertici tenendo
// conto degli smoothing groups; inoltre setta
// opportunamente i campi n1, n2, n3, sp1, sp2, sp3
// dei triangoli
if (vertexUV!=(AD_VectUV *)NULL) delete [] vertexUV;
if (bones_matrix!=(AD_Matrix **)NULL)
for (k=0; k<num_vertex3D; k++)
{
// questo perchè in fase di lettura (UtilsA3D) i punti nello
// spazio delle bonez vengono messi in points_tr
vect_copy(&vertex3D[k].tpoint, &vertex3D[k].point);
}
// NB: IL LOD VA INSERITO IN QUESTO PUNTO !!!!
// NE' PRIMA (perche' le uv ai vertici devono essere sistemate)
// NE' DOPO (perche' i tringoli devono eseere inizializzati
// tutti (anche quelli del modello semplificato) dopo)
// [ad]TURBO ROXXXXXXXXXXXXXXXXX
/*
lodder.apply_lod(vertex3D, num_vertex,
tria, num_tria,
70,
&vertex3D, &num_vertex,
&tria, &num_tria,
GEOMETRIC_LOD);
*/
split_tria_list();
return(0);
}
/**
* Main FastICA function. Centers and whitens the input
* matrix, calls the ICA computation function ICA_compute()
* and computes the output matrixes.
*/
void fastICA(mat X, int rows, int cols, int compc, mat K, mat W, mat A, mat S)
{
mat XT, V, TU, D, X1, _A;
vect scale, d;
// matrix creation
XT = mat_create(cols, rows);
X1 = mat_create(compc, rows);
V = mat_create(cols, cols);
D = mat_create(cols, cols);
TU = mat_create(cols, cols);
scale = vect_create(cols);
d = vect_create(cols);
/*
* CENTERING
*/
mat_center(X, rows, cols, scale);
/*
* WHITENING
*/
// X <- t(X); V <- X %*% t(X)/rows
mat_transpose(X, rows, cols, XT);
mat_apply_fx(X, rows, cols, fx_div_c, rows);
mat_mult(XT, cols, rows, X, rows, cols, V);
// La.svd(V)
svdcmp(V, cols, cols, d, D); // V = s$u, d = s$d, D = s$v
// D <- diag(c(1/sqrt(d))
vect_apply_fx(d, cols, fx_inv_sqrt, 0);
mat_diag(d, cols, D);
// K <- D %*% t(U)
mat_transpose(V, cols, cols, TU);
mat_mult(D, cols, cols, TU, cols, cols, V); // K = V
// X1 <- K %*% X
mat_mult(V, compc, cols, XT, cols, rows, X1);
/*
* FAST ICA
*/
_A = ICA_compute(X1, compc, rows);
/*
* OUTPUT
*/
// X <- t(x)
mat_transpose(XT, cols, rows, X);
mat_decenter(X, rows, cols, scale);
// K
mat_transpose(V, compc, cols, K);
// w <- a %*% K; S <- w %*% X
mat_mult(_A, compc, compc, V, compc, cols, D);
mat_mult(D, compc, cols, XT, cols, rows, X1);
// S
mat_transpose(X1, compc, rows, S);
// A <- t(w) %*% solve(w * t(w))
mat_transpose(D, compc, compc, TU);
mat_mult(D, compc, compc, TU, compc, compc, V);
mat_inverse(V, compc, D);
mat_mult(TU, compc, compc, D, compc, compc, V);
// A
mat_transpose(V, compc, compc, A);
// W
mat_transpose(_A, compc, compc, W);
// cleanup
mat_delete(XT, cols, rows);
mat_delete(X1, compc, rows);
mat_delete(V, cols, cols);
mat_delete(D, cols, cols);
mat_delete(TU,cols, cols);
vect_delete(scale);
vect_delete(d);
}
/**
* ICA function. Computes the W matrix from the
* preprocessed data.
*/
static mat ICA_compute(mat X, int rows, int cols)
{
mat TXp, GWX, W, Wd, W1, D, TU, TMP;
vect d, lim;
int i, it;
// matrix creation
TXp = mat_create(cols, rows);
GWX = mat_create(rows, cols);
W = mat_create(rows, rows);
Wd = mat_create(rows, rows);
D = mat_create(rows, rows);
TMP = mat_create(rows, rows);
TU = mat_create(rows, rows);
W1 = mat_create(rows, rows);
d = vect_create(rows);
// W rand init
mat_apply_fx(W, rows, rows, fx_rand, 0);
// sW <- La.svd(W)
mat_copy(W, rows, rows, Wd);
svdcmp(Wd, rows, rows, d, D);
// W <- sW$u %*% diag(1/sW$d) %*% t(sW$u) %*% W
mat_transpose(Wd, rows, rows, TU);
vect_apply_fx(d, rows, fx_inv, 0);
mat_diag(d, rows, D);
mat_mult(Wd, rows, rows, D, rows, rows, TMP);
mat_mult(TMP, rows, rows, TU, rows, rows, D);
mat_mult(D, rows, rows, W, rows, rows, Wd); // W = Wd
// W1 <- W
mat_copy(Wd, rows, rows, W1);
// lim <- rep(1000, maxit); it = 1
lim = vect_create(MAX_ITERATIONS);
for (i=0; i<MAX_ITERATIONS; i++)
lim[i] = 1000;
it = 0;
// t(X)/p
mat_transpose(X, rows, cols, TXp);
mat_apply_fx(TXp, cols, rows, fx_div_c, cols);
while (lim[it] > TOLERANCE && it < MAX_ITERATIONS) {
// wx <- W %*% X
mat_mult(Wd, rows, rows, X, rows, cols, GWX);
// gwx <- tanh(alpha * wx)
mat_apply_fx(GWX, rows, cols, fx_tanh, 0);
// v1 <- gwx %*% t(X)/p
mat_mult(GWX, rows, cols, TXp, cols, rows, TMP); // V1 = TMP
// g.wx <- alpha * (1 - (gwx)^2)
mat_apply_fx(GWX, rows, cols, fx_1sub_sqr, 0);
// v2 <- diag(apply(g.wx, 1, FUN = mean)) %*% W
mat_mean_rows(GWX, rows, cols, d);
mat_diag(d, rows, D);
mat_mult(D, rows, rows, Wd, rows, rows, TU); // V2 = TU
// W1 <- v1 - v2
mat_sub(TMP, TU, rows, rows, W1);
// sW1 <- La.svd(W1)
mat_copy(W1, rows, rows, W);
svdcmp(W, rows, rows, d, D);
// W1 <- sW1$u %*% diag(1/sW1$d) %*% t(sW1$u) %*% W1
mat_transpose(W, rows, rows, TU);
vect_apply_fx(d, rows, fx_inv, 0);
mat_diag(d, rows, D);
mat_mult(W, rows, rows, D, rows, rows, TMP);
mat_mult(TMP, rows, rows, TU, rows, rows, D);
mat_mult(D, rows, rows, W1, rows, rows, W); // W1 = W
// lim[it + 1] <- max(Mod(Mod(diag(W1 %*% t(W))) - 1))
mat_transpose(Wd, rows, rows, TU);
mat_mult(W, rows, rows, TU, rows, rows, TMP);
lim[it+1] = fabs(mat_max_diag(TMP, rows, rows) - 1);
// W <- W1
mat_copy(W, rows, rows, Wd);
it++;
}
// clean up
mat_delete(TXp, cols, rows);
mat_delete(GWX, rows, cols);
mat_delete(W, rows, rows);
mat_delete(D, rows, rows);
mat_delete(TMP, rows, rows);
mat_delete(TU, rows, rows);
mat_delete(W1, rows, rows);
vect_delete(d);
return Wd;
}
void AD_FFDModifier::update(float4 framepos)
{
int w;
AD_Vect3D postmp, stmp;
AD_Quaternion objrot;
AD_Matrix posttrans, scaling, maux, mrot;
AD_Matrix inv_posttrans, inv_scaling, inv_mrot;
AD_Vect3D size;
// Matrice di rotazione e la sua inversa
mat_identity(&mrot);
if (rotationtrack.numkey>0)
{
rotationtrack.get_data(framepos, &objrot);
quat_rotquat_to_matrix(&objrot, &mrot);
}
mat_copy(&mrot, &latticematrix);
mat_transpose(&mrot, &inv_mrot);
// Matrice di scaling e la sua inversa
if (scaletrack.numkey>0)
{
scaletrack.get_data(framepos, &stmp);
mat_setmatrix_of_scaling(&scaling, stmp.x, stmp.y, stmp.z);
mat_setmatrix_of_scaling(&inv_scaling, 1.0f/stmp.x, 1.0f/stmp.y, 1.0f/stmp.z);
}
else
{
mat_identity(&scaling);
mat_identity(&inv_scaling);
}
// Matrice di traslazione e la sua inversa
if (positiontrack.numkey>0) positiontrack.get_data(framepos, ¤tpos);
postmp.x=-currentpos.x;
postmp.y=-currentpos.y;
postmp.z=-currentpos.z;
mat_setmatrix_of_pretraslation(&posttrans, ¤tpos);
mat_setmatrix_of_pretraslation(&inv_posttrans, &postmp);
// Costruzione matrice lattice
mat_mul(&scaling, &mrot, &maux);
mat_mul(&posttrans, &maux, &latticematrix);
// Costruzione matrice inversa del lattice
mat_mul(&inv_scaling, &inv_posttrans, &maux);
mat_mul(&inv_mrot, &maux, &inv_latticematrix);
// Costruzione della size: servirà per portare nello spazio
// normalizzato (0..1) del lattice il punto da mappare
size.x=bbx2-bbx1; if (size.x==0) size.x=0.001f;
size.y=bby2-bby1; if (size.y==0) size.y=0.001f;
size.z=bbz2-bbz1; if (size.z==0) size.z=0.001f;
mat_setmatrix_of_scaling(&scaling, size.x, size.y, size.z);
mat_setmatrix_of_scaling(&inv_scaling, 1.0f/size.x, 1.0f/size.y, 1.0f/size.z);
// L'origine della TM è l'angolo in basso a sx della bounding
// box, non il centro.
stmp.x=currentpos.x-bbx1;
stmp.y=currentpos.y-bby1;
stmp.z=currentpos.z-bbz1;
mat_setmatrix_of_pretraslation(&posttrans, &stmp);
stmp.x=-stmp.x;
stmp.y=-stmp.y;
stmp.z=-stmp.z;
mat_setmatrix_of_pretraslation(&inv_posttrans, &stmp);
// Costruzione di tm, e invtm
mat_mul(&inv_scaling, &posttrans, &maux);
mat_mul(&maux, &inv_latticematrix, &tm);
mat_mul(&inv_posttrans, &scaling, &maux);
mat_mul(&latticematrix, &maux, &invtm);
for (w=0; w<num_cpoints; w++)
{
if (cpoints_tracks[w].numkey>0) cpoints_tracks[w].get_data(framepos, &cpoints[w]);
}
}
void frob(const mpoly_t P, const ctx_t ctxFracQt,
const qadic_t t1, const qadic_ctx_t Qq,
prec_t *prec, const prec_t *prec_in,
int verbose)
{
const padic_ctx_struct *Qp = &Qq->pctx;
const fmpz *p = Qp->p;
const long a = qadic_ctx_degree(Qq);
const long n = P->n - 1;
const long d = mpoly_degree(P, -1, ctxFracQt);
const long b = gmc_basis_size(n, d);
long i, j, k;
/* Diagonal fibre */
padic_mat_t F0;
/* Gauss--Manin Connection */
mat_t M;
mon_t *bR, *bC;
fmpz_poly_t r;
/* Local solution */
fmpz_poly_mat_t C, Cinv;
long vC, vCinv;
/* Frobenius */
fmpz_poly_mat_t F;
long vF;
fmpz_poly_mat_t F1;
long vF1;
fmpz_poly_t cp;
clock_t c0, c1;
double c;
if (verbose)
{
printf("Input:\n");
printf(" P = "), mpoly_print(P, ctxFracQt), printf("\n");
printf(" p = "), fmpz_print(p), printf("\n");
printf(" t1 = "), qadic_print_pretty(t1, Qq), printf("\n");
printf("\n");
fflush(stdout);
}
/* Step 1 {M, r} *********************************************************/
c0 = clock();
mat_init(M, b, b, ctxFracQt);
fmpz_poly_init(r);
gmc_compute(M, &bR, &bC, P, ctxFracQt);
{
fmpz_poly_t t;
fmpz_poly_init(t);
fmpz_poly_set_ui(r, 1);
for (i = 0; i < M->m; i++)
for (j = 0; j < M->n; j++)
{
fmpz_poly_lcm(t, r, fmpz_poly_q_denref(
(fmpz_poly_q_struct *) mat_entry(M, i, j, ctxFracQt)));
fmpz_poly_swap(r, t);
}
fmpz_poly_clear(t);
}
c1 = clock();
c = (double) (c1 - c0) / CLOCKS_PER_SEC;
if (verbose)
{
printf("Gauss-Manin connection:\n");
printf(" r(t) = "), fmpz_poly_print_pretty(r, "t"), printf("\n");
printf(" Time = %f\n", c);
printf("\n");
fflush(stdout);
}
{
qadic_t t;
qadic_init2(t, 1);
fmpz_poly_evaluate_qadic(t, r, t1, Qq);
if (qadic_is_zero(t))
{
printf("Exception (deformation_frob).\n");
printf("The resultant r evaluates to zero (mod p) at t1.\n");
abort();
}
qadic_clear(t);
}
/* Precisions ************************************************************/
if (prec_in != NULL)
{
*prec = *prec_in;
}
else
{
deformation_precisions(prec, p, a, n, d, fmpz_poly_degree(r));
}
if (verbose)
{
printf("Precisions:\n");
printf(" N0 = %ld\n", prec->N0);
printf(" N1 = %ld\n", prec->N1);
printf(" N2 = %ld\n", prec->N2);
printf(" N3 = %ld\n", prec->N3);
printf(" N3i = %ld\n", prec->N3i);
printf(" N3w = %ld\n", prec->N3w);
printf(" N3iw = %ld\n", prec->N3iw);
printf(" N4 = %ld\n", prec->N4);
printf(" m = %ld\n", prec->m);
printf(" K = %ld\n", prec->K);
printf(" r = %ld\n", prec->r);
printf(" s = %ld\n", prec->s);
printf("\n");
fflush(stdout);
}
/* Initialisation ********************************************************/
padic_mat_init2(F0, b, b, prec->N4);
fmpz_poly_mat_init(C, b, b);
fmpz_poly_mat_init(Cinv, b, b);
fmpz_poly_mat_init(F, b, b);
vF = 0;
fmpz_poly_mat_init(F1, b, b);
vF1 = 0;
fmpz_poly_init(cp);
/* Step 2 {F0} ***********************************************************/
{
padic_ctx_t pctx_F0;
fmpz *t;
padic_ctx_init(pctx_F0, p, FLINT_MIN(prec->N4 - 10, 0), prec->N4, PADIC_VAL_UNIT);
t = _fmpz_vec_init(n + 1);
c0 = clock();
mpoly_diagonal_fibre(t, P, ctxFracQt);
diagfrob(F0, t, n, d, prec->N4, pctx_F0, 0);
padic_mat_transpose(F0, F0);
c1 = clock();
c = (double) (c1 - c0) / CLOCKS_PER_SEC;
if (verbose)
{
printf("Diagonal fibre:\n");
printf(" P(0) = {"), _fmpz_vec_print(t, n + 1), printf("}\n");
printf(" Time = %f\n", c);
printf("\n");
fflush(stdout);
}
_fmpz_vec_clear(t, n + 1);
padic_ctx_clear(pctx_F0);
}
/* Step 3 {C, Cinv} ******************************************************/
/*
Compute C as a matrix over Z_p[[t]]. A is the same but as a series
of matrices over Z_p. Mt is the matrix -M^t, and Cinv is C^{-1}^t,
the local solution of the differential equation replacing M by Mt.
*/
c0 = clock();
{
const long K = prec->K;
padic_mat_struct *A;
gmde_solve(&A, K, p, prec->N3, prec->N3w, M, ctxFracQt);
gmde_convert_soln(C, &vC, A, K, p);
for(i = 0; i < K; i++)
padic_mat_clear(A + i);
free(A);
}
c1 = clock();
c = (double) (c1 - c0) / CLOCKS_PER_SEC;
if (verbose)
{
printf("Local solution:\n");
printf(" Time for C = %f\n", c);
fflush(stdout);
}
c0 = clock();
{
const long K = (prec->K + (*p) - 1) / (*p);
mat_t Mt;
padic_mat_struct *Ainv;
mat_init(Mt, b, b, ctxFracQt);
mat_transpose(Mt, M, ctxFracQt);
mat_neg(Mt, Mt, ctxFracQt);
gmde_solve(&Ainv, K, p, prec->N3i, prec->N3iw, Mt, ctxFracQt);
gmde_convert_soln(Cinv, &vCinv, Ainv, K, p);
fmpz_poly_mat_transpose(Cinv, Cinv);
fmpz_poly_mat_compose_pow(Cinv, Cinv, *p);
for(i = 0; i < K; i++)
padic_mat_clear(Ainv + i);
free(Ainv);
mat_clear(Mt, ctxFracQt);
}
c1 = clock();
c = (double) (c1 - c0) / CLOCKS_PER_SEC;
if (verbose)
{
printf(" Time for C^{-1} = %f\n", c);
printf("\n");
fflush(stdout);
}
/* Step 4 {F(t) := C(t) F(0) C(t^p)^{-1}} ********************************/
/*
Computes the product C(t) F(0) C(t^p)^{-1} modulo (p^{N_2}, t^K).
This is done by first computing the unit part of the product
exactly over the integers modulo t^K.
*/
c0 = clock();
{
fmpz_t pN;
fmpz_poly_mat_t T;
fmpz_init(pN);
fmpz_poly_mat_init(T, b, b);
for (i = 0; i < b; i++)
{
/* Find the unique k s.t. F0(i,k) is non-zero */
for (k = 0; k < b; k++)
if (!fmpz_is_zero(padic_mat_entry(F0, i, k)))
break;
if (k == b)
{
printf("Exception (frob). F0 is singular.\n\n");
abort();
}
for (j = 0; j < b; j++)
{
fmpz_poly_scalar_mul_fmpz(fmpz_poly_mat_entry(T, i, j),
fmpz_poly_mat_entry(Cinv, k, j),
padic_mat_entry(F0, i, k));
}
}
fmpz_poly_mat_mul(F, C, T);
fmpz_poly_mat_truncate(F, prec->K);
vF = vC + padic_mat_val(F0) + vCinv;
/* Canonicalise (F, vF) */
{
long v = fmpz_poly_mat_ord_p(F, p);
if (v == LONG_MAX)
{
printf("ERROR (deformation_frob). F(t) == 0.\n");
abort();
}
else if (v > 0)
{
fmpz_pow_ui(pN, p, v);
fmpz_poly_mat_scalar_divexact_fmpz(F, F, pN);
vF = vF + v;
}
}
/* Reduce (F, vF) modulo p^{N2} */
fmpz_pow_ui(pN, p, prec->N2 - vF);
fmpz_poly_mat_scalar_mod_fmpz(F, F, pN);
fmpz_clear(pN);
fmpz_poly_mat_clear(T);
}
c1 = clock();
c = (double) (c1 - c0) / CLOCKS_PER_SEC;
if (verbose)
{
printf("Matrix for F(t):\n");
printf(" Time = %f\n", c);
printf("\n");
fflush(stdout);
}
/* Step 5 {G = r(t)^m F(t)} **********************************************/
c0 = clock();
{
fmpz_t pN;
fmpz_poly_t t;
fmpz_init(pN);
fmpz_poly_init(t);
fmpz_pow_ui(pN, p, prec->N2 - vF);
/* Compute r(t)^m mod p^{N2-vF} */
if (prec->denR == NULL)
{
fmpz_mod_poly_t _t;
fmpz_mod_poly_init(_t, pN);
fmpz_mod_poly_set_fmpz_poly(_t, r);
fmpz_mod_poly_pow(_t, _t, prec->m);
fmpz_mod_poly_get_fmpz_poly(t, _t);
fmpz_mod_poly_clear(_t);
}
else
{
/* TODO: We don't really need a copy */
fmpz_poly_set(t, prec->denR);
}
fmpz_poly_mat_scalar_mul_fmpz_poly(F, F, t);
fmpz_poly_mat_scalar_mod_fmpz(F, F, pN);
/* TODO: This should not be necessary? */
fmpz_poly_mat_truncate(F, prec->K);
fmpz_clear(pN);
fmpz_poly_clear(t);
}
c1 = clock();
c = (double) (c1 - c0) / CLOCKS_PER_SEC;
if (verbose)
{
printf("Analytic continuation:\n");
printf(" Time = %f\n", c);
printf("\n");
fflush(stdout);
}
/* Steps 6 and 7 *********************************************************/
if (a == 1)
{
/* Step 6 {F(1) = r(t_1)^{-m} G(t_1)} ********************************/
c0 = clock();
{
const long N = prec->N2 - vF;
fmpz_t f, g, t, pN;
fmpz_init(f);
fmpz_init(g);
fmpz_init(t);
fmpz_init(pN);
fmpz_pow_ui(pN, p, N);
/* f := \hat{t_1}, g := r(\hat{t_1})^{-m} */
_padic_teichmuller(f, t1->coeffs + 0, p, N);
if (prec->denR == NULL)
{
_fmpz_mod_poly_evaluate_fmpz(g, r->coeffs, r->length, f, pN);
fmpz_powm_ui(t, g, prec->m, pN);
}
else
{
_fmpz_mod_poly_evaluate_fmpz(t, prec->denR->coeffs, prec->denR->length, f, pN);
}
_padic_inv(g, t, p, N);
/* F1 := g G(\hat{t_1}) */
for (i = 0; i < b; i++)
for (j = 0; j < b; j++)
{
const fmpz_poly_struct *poly = fmpz_poly_mat_entry(F, i, j);
const long len = poly->length;
if (len == 0)
{
fmpz_poly_zero(fmpz_poly_mat_entry(F1, i, j));
}
else
{
fmpz_poly_fit_length(fmpz_poly_mat_entry(F1, i, j), 1);
_fmpz_mod_poly_evaluate_fmpz(t, poly->coeffs, len, f, pN);
fmpz_mul(fmpz_poly_mat_entry(F1, i, j)->coeffs + 0, g, t);
fmpz_mod(fmpz_poly_mat_entry(F1, i, j)->coeffs + 0,
fmpz_poly_mat_entry(F1, i, j)->coeffs + 0, pN);
_fmpz_poly_set_length(fmpz_poly_mat_entry(F1, i, j), 1);
_fmpz_poly_normalise(fmpz_poly_mat_entry(F1, i, j));
}
}
vF1 = vF;
fmpz_poly_mat_canonicalise(F1, &vF1, p);
fmpz_clear(f);
fmpz_clear(g);
fmpz_clear(t);
fmpz_clear(pN);
}
c1 = clock();
c = (double) (c1 - c0) / CLOCKS_PER_SEC;
if (verbose)
{
printf("Evaluation:\n");
printf(" Time = %f\n", c);
printf("\n");
fflush(stdout);
}
}
else
{
/* Step 6 {F(1) = r(t_1)^{-m} G(t_1)} ********************************/
c0 = clock();
{
const long N = prec->N2 - vF;
fmpz_t pN;
fmpz *f, *g, *t;
fmpz_init(pN);
f = _fmpz_vec_init(a);
g = _fmpz_vec_init(2 * a - 1);
t = _fmpz_vec_init(2 * a - 1);
fmpz_pow_ui(pN, p, N);
/* f := \hat{t_1}, g := r(\hat{t_1})^{-m} */
_qadic_teichmuller(f, t1->coeffs, t1->length, Qq->a, Qq->j, Qq->len, p, N);
if (prec->denR == NULL)
{
fmpz_t e;
fmpz_init_set_ui(e, prec->m);
_fmpz_mod_poly_compose_smod(g, r->coeffs, r->length, f, a,
Qq->a, Qq->j, Qq->len, pN);
_qadic_pow(t, g, a, e, Qq->a, Qq->j, Qq->len, pN);
fmpz_clear(e);
}
else
{
_fmpz_mod_poly_reduce(prec->denR->coeffs, prec->denR->length, Qq->a, Qq->j, Qq->len, pN);
_fmpz_poly_normalise(prec->denR);
_fmpz_mod_poly_compose_smod(t, prec->denR->coeffs, prec->denR->length, f, a,
Qq->a, Qq->j, Qq->len, pN);
}
_qadic_inv(g, t, a, Qq->a, Qq->j, Qq->len, p, N);
/* F1 := g G(\hat{t_1}) */
for (i = 0; i < b; i++)
for (j = 0; j < b; j++)
{
const fmpz_poly_struct *poly = fmpz_poly_mat_entry(F, i, j);
const long len = poly->length;
fmpz_poly_struct *poly2 = fmpz_poly_mat_entry(F1, i, j);
if (len == 0)
{
fmpz_poly_zero(poly2);
}
else
{
_fmpz_mod_poly_compose_smod(t, poly->coeffs, len, f, a,
Qq->a, Qq->j, Qq->len, pN);
fmpz_poly_fit_length(poly2, 2 * a - 1);
_fmpz_poly_mul(poly2->coeffs, g, a, t, a);
_fmpz_mod_poly_reduce(poly2->coeffs, 2 * a - 1, Qq->a, Qq->j, Qq->len, pN);
_fmpz_poly_set_length(poly2, a);
_fmpz_poly_normalise(poly2);
}
}
/* Now the matrix for p^{-1} F_p at t=t_1 is (F1, vF1). */
vF1 = vF;
fmpz_poly_mat_canonicalise(F1, &vF1, p);
fmpz_clear(pN);
_fmpz_vec_clear(f, a);
_fmpz_vec_clear(g, 2 * a - 1);
_fmpz_vec_clear(t, 2 * a - 1);
}
c1 = clock();
c = (double) (c1 - c0) / CLOCKS_PER_SEC;
if (verbose)
{
printf("Evaluation:\n");
printf(" Time = %f\n", c);
printf("\n");
fflush(stdout);
}
/* Step 7 {Norm} *****************************************************/
/*
Computes the matrix for $q^{-1} F_q$ at $t = t_1$ as the
product $F \sigma(F) \dotsm \sigma^{a-1}(F)$ up appropriate
transpositions because our convention of columns vs rows is
the opposite of that used by Gerkmann.
Note that, in any case, transpositions do not affect
the characteristic polynomial.
*/
c0 = clock();
{
const long N = prec->N1 - a * vF1;
fmpz_t pN;
fmpz_poly_mat_t T;
fmpz_init(pN);
fmpz_poly_mat_init(T, b, b);
fmpz_pow_ui(pN, p, N);
fmpz_poly_mat_frobenius(T, F1, 1, p, N, Qq);
_qadic_mat_mul(F1, F1, T, pN, Qq);
for (i = 2; i < a; i++)
{
fmpz_poly_mat_frobenius(T, T, 1, p, N, Qq);
_qadic_mat_mul(F1, F1, T, pN, Qq);
}
vF1 = a * vF1;
fmpz_poly_mat_canonicalise(F1, &vF1, p);
fmpz_clear(pN);
fmpz_poly_mat_clear(T);
}
c1 = clock();
c = (double) (c1 - c0) / CLOCKS_PER_SEC;
if (verbose)
{
printf("Norm:\n");
printf(" Time = %f\n", c);
printf("\n");
fflush(stdout);
}
}
/* Step 8 {Reverse characteristic polynomial} ****************************/
c0 = clock();
deformation_revcharpoly(cp, F1, vF1, n, d, prec->N0, prec->r, prec->s, Qq);
c1 = clock();
c = (double) (c1 - c0) / CLOCKS_PER_SEC;
if (verbose)
{
printf("Reverse characteristic polynomial:\n");
printf(" p(T) = "), fmpz_poly_print_pretty(cp, "T"), printf("\n");
printf(" Time = %f\n", c);
printf("\n");
fflush(stdout);
}
/* Clean up **************************************************************/
padic_mat_clear(F0);
mat_clear(M, ctxFracQt);
free(bR);
free(bC);
fmpz_poly_clear(r);
fmpz_poly_mat_clear(C);
fmpz_poly_mat_clear(Cinv);
fmpz_poly_mat_clear(F);
fmpz_poly_mat_clear(F1);
fmpz_poly_clear(cp);
}
void AD_BendModifier::update(float4 framepos)
{
AD_Vect3D sub_center;
AD_Matrix mrot, pret, ipret, mr_plus;
AD_Matrix mrot2, imrot2;
float raux;
if (center_track!=(postrack *)NULL)
center_track->get_data(framepos, ¢er);
if (angle_track!=(rolltrack *)NULL)
angle_track->get_data(framepos, &angle);
if (dir_track!=(rolltrack *)NULL)
dir_track->get_data(framepos, &dir);
if (uplim_track!=(rolltrack *)NULL)
uplim_track->get_data(framepos, &uplim);
if (lowlim_track!=(rolltrack *)NULL)
lowlim_track->get_data(framepos, &lowlim);
// costruzione matrice di trasformazione e la sua
// inversa
sub_center.x=-center.x;
sub_center.y=-center.y;
sub_center.z=-center.z;
mat_setmatrix_of_eulerrotationY(&mr_plus, dir);
switch (axis)
{
case 0: // asse X
mat_setmatrix_of_eulerrotationZ(&mrot, (float)(-M_PI/2.0));
raux=bbx2-bbx1;
break;
case 1: // asse Y
mat_identity(&mrot);
raux=bby2-bby1;
break;
case 2: // asse Z
mat_setmatrix_of_eulerrotationX(&mrot, (float)(M_PI/2.0));
raux=bbz2-bbz1;
break;
}
mat_mul(&mr_plus, &mrot, &mrot2);
mat_transpose(&mrot2, &imrot2);
if (dolim) raux=uplim-lowlim;
if (fabs(angle) <0.0001)
r = 0;
else r = raux/angle;
mat_setmatrix_of_pretraslation(&pret, &sub_center);
mat_setmatrix_of_pretraslation(&ipret, ¢er);
mat_mul (&mrot2, &pret, &tm);
mat_mul (&ipret, &imrot2, &invtm);
float len = uplim-lowlim;
float rat1, rat2;
// AD_Vect3D pt;
if (len==0.0f)
{
rat1 = rat2 = 1.0f;
}
else
{
rat1 = uplim/len;
rat2 = lowlim/len;
}
/*
tmAbove.IdentityMatrix();
tmAbove.Translate(Point3(0.0f,0.0f,-to));
tmAbove.RotateY(DegToRad(angle * rat1));
tmAbove.Translate(Point3(0.0f,0.0f,to));
pt = Point3(0.0f,0.0f,to);
tmAbove.Translate((Map(0,pt*invtm)*tm)-pt);
tmBelow.IdentityMatrix();
tmBelow.Translate(Point3(0.0f,0.0f,-from));
tmBelow.RotateY(DegToRad(angle * rat2));
tmBelow.Translate(Point3(0.0f,0.0f,from));
pt = Point3(0.0f,0.0f,from);
tmBelow.Translate((Map(0,pt*invtm)*tm)-pt);
*/
}
/**
* ICA function. Computes the W matrix from the
* preprocessed data.
*/
static mat ICA_compute(mat X, int rows, int cols) {
mat TXp, GWX, W, Wd, W1, D, TU, TMP;
vect d, lim;
int i, it;
FILE *OutputFile;
clock_t clock1, clock2;
float time;
//char ascii_path[512];
//strcpy(ascii_path, "/storage/sdcard0/NickGun/EEG/Log.txt");
//FILE *Log;
// matrix creation
TXp = mat_create(cols, rows);
GWX = mat_create(rows, cols);
W = mat_create(rows, rows);
Wd = mat_create(rows, rows);
D = mat_create(rows, rows);
TMP = mat_create(rows, rows);
TU = mat_create(rows, rows);
W1 = mat_create(rows, rows);
d = vect_create(rows);
// W rand init
mat_apply_fx(W, rows, rows, fx_rand, 0);
// sW <- La.svd(W)
mat_copy(W, rows, rows, Wd);
svdcmp(Wd, rows, rows, d, D);
// W <- sW$u %*% diag(1/sW$d) %*% t(sW$u) %*% W
mat_transpose(Wd, rows, rows, TU);
vect_apply_fx(d, rows, fx_inv, 0);
mat_diag(d, rows, D);
mat_mult(Wd, rows, rows, D, rows, rows, TMP);
mat_mult(TMP, rows, rows, TU, rows, rows, D);
mat_mult(D, rows, rows, W, rows, rows, Wd); // W = Wd
// W1 <- W
mat_copy(Wd, rows, rows, W1);
// lim <- rep(1000, maxit); it = 1
lim = vect_create(MAX_ITERATIONS);
for (i = 0; i < MAX_ITERATIONS; i++)
lim[i] = 1000;
it = 0;
// t(X)/p
mat_transpose(X, rows, cols, TXp);
mat_apply_fx(TXp, cols, rows, fx_div_c, cols);
while (lim[it] > TOLERANCE && it < MAX_ITERATIONS) {
// wx <- W %*% X
mat_mult(Wd, rows, rows, X, rows, cols, GWX);
// gwx <- tanh(alpha * wx)
mat_apply_fx(GWX, rows, cols, fx_tanh, 0);
// v1 <- gwx %*% t(X)/p
mat_mult(GWX, rows, cols, TXp, cols, rows, TMP); // V1 = TMP
// g.wx <- alpha * (1 - (gwx)^2)
mat_apply_fx(GWX, rows, cols, fx_1sub_sqr, 0);
// v2 <- diag(apply(g.wx, 1, FUN = mean)) %*% W
mat_mean_rows(GWX, rows, cols, d);
mat_diag(d, rows, D);
mat_mult(D, rows, rows, Wd, rows, rows, TU); // V2 = TU
// W1 <- v1 - v2
mat_sub(TMP, TU, rows, rows, W1);
// sW1 <- La.svd(W1)
mat_copy(W1, rows, rows, W);
svdcmp(W, rows, rows, d, D);
// W1 <- sW1$u %*% diag(1/sW1$d) %*% t(sW1$u) %*% W1
mat_transpose(W, rows, rows, TU);
vect_apply_fx(d, rows, fx_inv, 0);
mat_diag(d, rows, D);
mat_mult(W, rows, rows, D, rows, rows, TMP);
mat_mult(TMP, rows, rows, TU, rows, rows, D);
mat_mult(D, rows, rows, W1, rows, rows, W); // W1 = W
// lim[it + 1] <- max(Mod(Mod(diag(W1 %*% t(W))) - 1))
mat_transpose(Wd, rows, rows, TU); //chuyen vi
mat_mult(W, rows, rows, TU, rows, rows, TMP); //TMP=WxTU
lim[it + 1] = fabs(mat_max_diag(TMP, rows, rows) - 1);
if(lim[it+1]<0.1)
break;
/*
OutputFile = fopen("/storage/sdcard0/Nickgun/EEG/data/lim",
"at");
fprintf(OutputFile, "%f \n", lim[it+1]);
fclose(OutputFile);
// W <- W1
*/
mat_copy(W, rows, rows, Wd);
it++;
}
// clean up
mat_delete(TXp, cols, rows);
mat_delete(GWX, rows, cols);
mat_delete(W, rows, rows);
mat_delete(D, rows, rows);
mat_delete(TMP, rows, rows);
mat_delete(TU, rows, rows);
mat_delete(W1, rows, rows);
vect_delete(d);
return Wd;
}
/**
* Main FastICA function. Centers and whitens the input
* matrix, calls the ICA computation function ICA_compute()
* and computes the output matrixes.
*/
void fastICA(mat X, int rows, int cols, int compc, mat K, mat W, mat A, mat S) {
mat XT, V, TU, D, X1, _A;
vect scale, d;
clock_t clock1, clock2;
float time;
//char ascii_path[512];
//strcpy(ascii_path, "/storage/sdcard0/NickGun/EEG/Log.txt");
//FILE *Log;
//chu thich voi truong hop 14 kenh, 2s (256mau) du lieu, 14 thanh phan doc lap>>> cols = 14, rows = 256, compc = 14
// matrix creation
XT = mat_create(cols, rows); //14x256
X1 = mat_create(compc, rows); //14x256
V = mat_create(cols, cols); //14x14
D = mat_create(cols, cols); //14x14
TU = mat_create(cols, cols); //14x14
scale = vect_create(cols); //14
d = vect_create(cols); //14
clock1 = clock();
/*
* CENTERING
*/
mat_center(X, rows, cols, scale); //tru di gia tri trung binh cua moi cot
clock2 = clock();
time = (clock2 - clock1) / CLOCKS_PER_SEC;
//Log = fopen(ascii_path, "wb");
//fprintf(Log, "CENTERING %f \n", time);
//fclose(Log);
clock1 = clock();
/*
* WHITENING
*/
// X <- t(X); V <- X %*% t(X)/rows
mat_transpose(X, rows, cols, XT); //XT la chuyen vi cua ma tran X[256][14] >>> XT[14][256]
mat_apply_fx(X, rows, cols, fx_div_c, rows); //lay tung gia tri cua X[i][j] chia cho 14
mat_mult(XT, cols, rows, X, rows, cols, V); //V=XT*X >>>V[14][14]
// La.svd(V)
svdcmp(V, cols, cols, d, D); // V = s$u, d = s$d, D = s$v
// D <- diag(c(1/sqrt(d))
vect_apply_fx(d, cols, fx_inv_sqrt, 0);
mat_diag(d, cols, D);
// K <- D %*% t(U)
mat_transpose(V, cols, cols, TU);
mat_mult(D, cols, cols, TU, cols, cols, V); // K = V
// X1 <- K %*% X
mat_mult(V, compc, cols, XT, cols, rows, X1);
clock2 = clock();
time = (clock2 - clock1) / CLOCKS_PER_SEC;
//Log = fopen(ascii_path, "at");
//fprintf(Log, "WHITENING %f \n", time);
//fclose(Log);
clock1 = clock();
/*
* FAST ICA
*/
_A = ICA_compute(X1, compc, rows);
clock2 = clock();
time = (clock2 - clock1) / CLOCKS_PER_SEC;
//Log = fopen(ascii_path, "at");
//fprintf(Log, "FASTICA %f \n", time);
//fclose(Log);
clock1 = clock();
/*
* OUTPUT
*/
// X <- t(x)
mat_transpose(XT, cols, rows, X);
mat_decenter(X, rows, cols, scale);
// K
mat_transpose(V, compc, cols, K);
// w <- a %*% K; S <- w %*% X
mat_mult(_A, compc, compc, V, compc, cols, D);
mat_mult(D, compc, cols, XT, cols, rows, X1);
// S
mat_transpose(X1, compc, rows, S);
// A <- t(w) %*% solve(w * t(w))
mat_transpose(D, compc, compc, TU);
mat_mult(D, compc, compc, TU, compc, compc, V);
mat_inverse(V, compc, D); //ham nay tinh mat tran ngich dao
mat_mult(TU, compc, compc, D, compc, compc, V);
// A
mat_transpose(V, compc, compc, A);
// W
mat_transpose(_A, compc, compc, W);
// cleanup
mat_delete(XT, cols, rows);
mat_delete(X1, compc, rows);
mat_delete(V, cols, cols);
mat_delete(D, cols, cols);
mat_delete(TU, cols, cols);
vect_delete(scale);
vect_delete(d);
clock2 = clock();
time = (clock2 - clock1) / CLOCKS_PER_SEC;
//Log = fopen(ascii_path, "at");
//fprintf(Log, "OUTPUT %f \n", time);
//fclose(Log);
}
void AD_Object3D::build_objectmatrix (float4 framepos)
// costruisce la matrice di trasformazione, che servira' poi per trasformare
// i vertici dell'oggetto;
{
AD_Vect3D postmp, stmp, ptmp;
AD_Quaternion objrot;
AD_Matrix posttrans, scaling, maux, pretrans;
accum_scale.x=accum_scale.y=accum_scale.z=1.0f;
mat_identity(¤tmatrix_rot);
if (rotationtrack.numkey>0)
{
rotationtrack.get_data(framepos, &objrot);
quat_rotquat_to_matrix(&objrot, ¤tmatrix_rot);
}
mat_copy(¤tmatrix_rot, ¤tmatrix);
if (scaletrack.numkey>0)
{
scaletrack.get_data(framepos, &stmp);
mat_setmatrix_of_scaling(&scaling, stmp.x, stmp.y, stmp.z);
accum_scale.x*=stmp.x;
accum_scale.y*=stmp.y;
accum_scale.z*=stmp.z;
} else mat_identity(&scaling);
if (positiontrack.numkey>0) positiontrack.get_data(framepos, ¤tpos);
mat_setmatrix_of_pretraslation(&posttrans, ¤tpos);
if (type==BONE)
{
mat_mul(&scaling, ¤tmatrix_rot, &maux);
mat_mul(&posttrans, &maux, ¤tmatrix);
}
else
{
mat_mul(¤tmatrix_rot, &scaling, &maux);
mat_mul(&posttrans, &maux, ¤tmatrix);
}
// per le bone si costruiscono gia' le matrici comprese
// di padre; per oggetti non bone invece si costruiscono
// matrici relative e quindi devo 'accodarci' pure
// la trasformazione del padre
if (type!=BONE)
{
if (father!=(AD_Object3D *)NULL)
{
mat_mulaffine(&father->currentmatrix_rot, ¤tmatrix_rot, ¤tmatrix_rot);
mat_mul(&father->currentmatrix, ¤tmatrix, ¤tmatrix);
mat_mulvect(&father->currentmatrix, ¤tpos, &postmp);
vect_copy(&postmp, ¤tpos);
accum_scale.x*=father->accum_scale.x;
accum_scale.y*=father->accum_scale.y;
accum_scale.z*=father->accum_scale.z;
}
}
mat_transpose(¤tmatrix_rot, &inverse_rotmatrix);
if (skinned_object!=(AD_Object3D *)NULL)
{
if (father==(AD_Object3D *)NULL)
{
vect_copy(¤tpos, &ptmp);
vect_neg(&ptmp, &ptmp);
mat_setmatrix_of_pretraslation(&pretrans, &ptmp);
vect_neg(&ptmp, &ptmp);
mat_setmatrix_of_pretraslation(&posttrans, &ptmp);
mat_copy(&pretrans, &maux);
mat_mul(¤tmatrix_rot, &maux, &maux);
mat_mul(&rot0, &maux, &maux);
mat_mul(&scaling, &maux, &maux);
mat_setmatrix_of_scaling(&scaling, 1.0f/scale0.x, 1.0f/scale0.y, 1.0f/scale0.z);
mat_mul(&scaling, &maux, &maux);
//vect_sub(¤tpos, &pos0);
mat_mul(&posttrans, &maux, &skin_matrix);
}
else
{
/*
if (positiontrack.numkey>0) positiontrack.get_data(framepos, &ptmp);
vect_neg(&ptmp, &ptmp);
mat_setmatrix_of_pretraslation(&pretrans, &ptmp);
vect_neg(&ptmp, &ptmp);
mat_setmatrix_of_pretraslation(&posttrans, &ptmp);
mat_mul(&father->inverse_rotmatrix, ¤tmatrix_rot, &maux);
maux.a[0][3]=maux.a[1][3]=maux.a[2][3]=0;
mat_mul(&maux, &pretrans, &maux);
mat_mul(&scaling, &maux, &maux);
mat_mul(&posttrans, &maux, &skin_matrix);
mat_mul(&father->skin_matrix, &skin_matrix, &skin_matrix);
*/
mat_copy(&father->skin_matrix, &skin_matrix);
}
}
}
void AD_StretchModifier::update(float4 framepos)
{
AD_Vect3D sub_center;
AD_Matrix mrot, pret, ipret, imrot;
float amplify_aux;
if (center_track!=(postrack *)NULL)
center_track->get_data(framepos, ¢er);
if (stretch_track!=(rolltrack *)NULL)
stretch_track->get_data(framepos, &stretch);
if (amplify_track!=(rolltrack *)NULL)
amplify_track->get_data(framepos, &lify);
if (uplim_track!=(rolltrack *)NULL)
uplim_track->get_data(framepos, &uplim);
if (lowlim_track!=(rolltrack *)NULL)
lowlim_track->get_data(framepos, &lowlim);
// costruzione matrice di trasformazione e la sua
// inversa
sub_center.x=-center.x;
sub_center.y=-center.y;
sub_center.z=-center.z;
switch (axis)
{
case 0: // asse X
mat_setmatrix_of_eulerrotationZ(&mrot, (float)(-M_PI/2.0));
break;
case 1: // asse Y
mat_identity(&mrot);
break;
case 2: // asse Z
mat_setmatrix_of_eulerrotationX(&mrot, (float)(M_PI/2.0));
break;
}
mat_transpose(&mrot, &imrot);
mat_setmatrix_of_pretraslation(&pret, &sub_center);
mat_setmatrix_of_pretraslation(&ipret, ¢er);
mat_mul (&mrot, &pret, &tm);
mat_mul (&ipret, &imrot, &invtm);
amplify_aux = (amplify >= 0) ? amplify + 1 : 1.0f / (-amplify + 1.0f);
amplify=amplify_aux;
if (!dolim)
{
switch ( axis )
{
case 0:
heightMin = bbx1;
heightMax = bbx2;
break;
case 1:
heightMin = bby1;
heightMax = bby2;
break;
case 2:
heightMin = bbz1;
heightMax = bbz2;
break;
}
} else {
heightMin = lowlim;
heightMax = uplim;
}
return;
}
void * glclient_thread(void * arg)
{
server_thread_args_t * a = (server_thread_args_t *)arg;
static graphics_context_t gc;
static struct js_event joy;
int joy_fd;
static char button[32];
glclient_context_t *glcc = a->user_context_ptr;
joy_fd = open(glcc->joy_dev, O_RDONLY);
if (joy_fd == -1)
{
printf("Error: Joystick device open\n");
}
if (joy_fd != -1)
{
fcntl(joy_fd, F_SETFL, O_NONBLOCK);
}
gls_init(a);
gls_cmd_get_context();
gc.screen_width = glsc_global.screen_width;
gc.screen_height = glsc_global.screen_height;
printf("width:%d height:%d\n",glsc_global.screen_width,glsc_global.screen_height);
init_gl(&gc);
float aspect = (float)gc.screen_width / (float)gc.screen_height;
mat_perspective(proj_mat, aspect, 0.1f, 1024.0f, 60.0f);
glUniform4fv(gc.uloc_light, 1, light_pos);
srand(0x12345678);
int j;
for (j = 0; j < 1024; j++)
{
obj[j].z = randf() * 8.0f - 10.0f;
obj[j].x = (randf() * 2.0f - 1.0f) * obj[j].z;
obj[j].y = (randf() * 1.0f - 0.5f) * obj[j].z;
obj[j].dx = randf() * 0.0f - 0.0f;
obj[j].dy = randf() * 0.0f - 0.0f;
obj[j].dz = randf() * 0.0f - 0.0f;
obj[j].rx = randf() * 6.28;
obj[j].ry = randf() * 6.28;
obj[j].rz = randf() * 6.28;
obj[j].drx = randf() * 0.1f - 0.05f;
obj[j].dry = randf() * 0.1f - 0.05f;
obj[j].drz = randf() * 0.1f - 0.05f;
}
float x = 1.57f;
float y = 0.0f;
float z = -2.0f;
int k = 1;
int i;
for (i = 0; i < 432000; i++)
{
struct timeval times, timee;
gettimeofday(×, NULL);
if (joy_fd != -1)
{
while (read(joy_fd, &joy, sizeof(struct js_event)) == sizeof(struct js_event))
{
if ((joy.type & JS_EVENT_BUTTON) == JS_EVENT_BUTTON)
{
button[joy.number] = joy.value;
}
}
if (button[4] > 0)
{
y += 0.01f;
}
if (button[6] > 0)
{
y += -0.01f;
}
if (button[5] > 0)
{
x += 0.01f * aspect;
}
if (button[7] > 0)
{
x += -0.01f * aspect;
}
if (button[12] > 0)
{
z += -0.01f;
}
if (button[13] > 0)
{
k++;
k = (k > 45) ? 45 : k;
}
if (button[14] > 0)
{
z += 0.01f;
}
if (button[15] > 0)
{
k--;
k = (k < 1) ? 1 : k;
}
}
glUseProgram(gc.program);
glBindBuffer(GL_ARRAY_BUFFER, gc.vbo_pos);
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, gc.vbo_ind);
glEnableVertexAttribArray(gc.vloc_pos);
glEnableVertexAttribArray(gc.vloc_nor);
glEnableVertexAttribArray(gc.vloc_tex);
glClearColor(0.5f, 0.5f, 0.5f, 1.0f);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
for (j = 0; j < k; j++)
{
obj[j].rx += obj[j].drx;
obj[j].ry += obj[j].dry;
obj[j].rz += obj[j].drz;
if (j == 0)
{
obj[j].x = 0.0f;
obj[j].y = 0.0f;
obj[j].z = z;
obj[j].rx = -y;
obj[j].ry = x;
obj[j].rz = 0.0f;
}
mat_identity(model_mat);
mat_translate(model_mat, obj[j].x, obj[j].y, obj[j].z);
mat_rotate_x(model_mat, obj[j].rx);
mat_rotate_y(model_mat, obj[j].ry);
mat_rotate_z(model_mat, obj[j].rz);
mat_copy(nor_mat, model_mat);
mat_invert(nor_mat);
mat_transpose(nor_mat);
glUniformMatrix4fv(gc.uloc_nor, 1, GL_FALSE, nor_mat);
mat_copy(obj[j].nor_mat, nor_mat);
mat_copy(modelproj_mat, proj_mat);
mat_mul(modelproj_mat, model_mat);
glUniformMatrix4fv(gc.uloc_model, 1, GL_FALSE, modelproj_mat);
mat_copy(obj[j].modelproj_mat, modelproj_mat);
glDrawElements(GL_TRIANGLES, sizeof(ind_model) / sizeof(GLushort), GL_UNSIGNED_SHORT, 0);
}
glDisableVertexAttribArray(gc.vloc_tex);
glDisableVertexAttribArray(gc.vloc_nor);
glDisableVertexAttribArray(gc.vloc_pos);
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
glBindBuffer(GL_ARRAY_BUFFER, 0);
gls_cmd_flip(i);
gettimeofday(&timee, NULL);
//printf("%d:%f ms ", i, get_diff_time(times, timee) * 1000.0f);
}
glClearColor(0.0f, 0.0f, 0.0f, 1.0f);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
gls_cmd_flip(i);
release_gl(&gc);
gls_free();
if (joy_fd != -1)
{
close(joy_fd);
}
pthread_exit(NULL);
}
int
main(void)
{
int i, result;
flint_rand_t state;
printf("transpose... ");
fflush(stdout);
_randinit(state);
/* Check aliasing */
/* Unmanaged element type (long) */
for (i = 0; i < 100; i++)
{
long m, n;
ctx_t ctx;
mat_t A, B, C;
m = n_randint(state, 50) + 1;
n = m;
ctx_init_long(ctx);
mat_init(A, m, n, ctx);
mat_init(B, m, n, ctx);
mat_init(C, m, n, ctx);
mat_randtest(A, state, ctx);
mat_set(B, A, ctx);
mat_transpose(C, B, ctx);
mat_transpose(B, B, ctx);
result = mat_equal(B, C, ctx);
if (!result)
{
printf("FAIL:\n\n");
printf("Matrix A\n");
mat_print(A, ctx);
printf("\n");
printf("Matrix B\n");
mat_print(B, ctx);
printf("\n");
printf("Matrix C\n");
mat_print(C, ctx);
printf("\n");
}
mat_clear(A, ctx);
mat_clear(B, ctx);
mat_clear(C, ctx);
ctx_clear(ctx);
}
/* Check that (A^t)^t == A */
/* Unmanaged element type (long) */
for (i = 0; i < 100; i++)
{
long m, n;
ctx_t ctx;
mat_t A, B, C;
m = n_randint(state, 50) + 1;
n = n_randint(state, 50) + 1;
ctx_init_long(ctx);
mat_init(A, m, n, ctx);
mat_init(B, n, m, ctx);
mat_init(C, m, n, ctx);
mat_randtest(A, state, ctx);
mat_transpose(B, A, ctx);
mat_transpose(C, B, ctx);
result = mat_equal(A, C, ctx);
if (!result)
{
printf("FAIL:\n\n");
printf("Matrix A\n");
mat_print(A, ctx);
printf("\n");
printf("Matrix B\n");
mat_print(B, ctx);
printf("\n");
printf("Matrix C\n");
mat_print(C, ctx);
printf("\n");
}
mat_clear(A, ctx);
mat_clear(B, ctx);
mat_clear(C, ctx);
ctx_clear(ctx);
}
_randclear(state);
_fmpz_cleanup();
printf("PASS\n");
return EXIT_SUCCESS;
}
