/* ** 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; }