void CAnimating::GetBoneTransform( int iBone, matrix3x4_t &pBoneToWorld ) { CStudioHdr *pStudioHdr = GetModelPtr( ); if (!pStudioHdr) { Assert(!"CBaseAnimating::GetBoneTransform: model missing"); return; } if (iBone < 0 || iBone >= pStudioHdr->numbones()) { Assert(!"CBaseAnimating::GetBoneTransform: invalid bone index"); return; } CBoneCache *pcache = GetBoneCache( ); matrix3x4_t *pmatrix = pcache->GetCachedBone( iBone ); if ( !pmatrix ) { MatrixCopy( EntityToWorldTransform(), pBoneToWorld ); return; } Assert( pmatrix ); // FIXME MatrixCopy( *pmatrix, pBoneToWorld ); }
void MatrixStackPushMatrix (MatrixStack *stack, const s32 *ptr) { //printf("Push %i pos %i\n", stack->type, stack->position); if ((stack->type == 0) || (stack->type == 3)) MatrixCopy (&stack->matrix[0], ptr); else MatrixCopy (&stack->matrix[stack->position*16], ptr); MatrixStackSetStackPosition (stack, 1); }
void MatrixStackPopMatrix (s32 *mtxCurr, MatrixStack *stack, int size) { //printf("Pop %i pos %i (change %d)\n", stack->type, stack->position, -size); MatrixStackSetStackPosition(stack, -size); if ((stack->type == 0) || (stack->type == 3)) MatrixCopy (mtxCurr, &stack->matrix[0]); else MatrixCopy (mtxCurr, &stack->matrix[stack->position*16]); }
void MatrixMultiply2(matrix_t m, const matrix_t m2) { matrix_t tmp; MatrixCopy(m, tmp); MatrixMultiply(tmp, m2, m); }
qboolean MatrixInverse(matrix_t matrix) { float mdet = MatrixDet(matrix); matrix3x3_t mtemp; int i, j, sign; matrix_t m4x4_temp; #if 0 if ( fabs( mdet ) < 0.0000000001 ) return qtrue; #endif MatrixCopy(matrix, m4x4_temp); for ( i = 0; i < 4; i++ ) for ( j = 0; j < 4; j++ ) { sign = 1 - ( (i +j) % 2 ) * 2; m4_submat( m4x4_temp, mtemp, i, j ); // FIXME: try using * inverse det and see if speed/accuracy are good enough matrix[i+j*4] = ( m3_det( mtemp ) * sign ) / mdet; } return qfalse; }
/* ==================== StudioSaveBones ==================== */ void CStudioModelRenderer::StudioSaveBones( void ) { int i; mstudiobone_t *pbones; pbones = (mstudiobone_t *)((byte *)m_pStudioHeader + m_pStudioHeader->boneindex); m_nCachedBones = m_pStudioHeader->numbones; for (i = 0; i < m_pStudioHeader->numbones; i++) { strcpy( m_nCachedBoneNames[i], pbones[i].name ); MatrixCopy( (*m_pbonetransform)[i], m_rgCachedBoneTransform[i] ); MatrixCopy( (*m_plighttransform)[i], m_rgCachedLightTransform[i] ); } }
void InversMatrix(const int n, double **b, double **ib) { double **a; double *e; int i,j; int *p; a=MatrixAlloc(n); e=VectorAlloc(n); p=IntVectorAlloc(n); MatrixCopy(n, a, b); LUfact(n, a, p); for(i=0; i<n; i++) { for(j=0; j<n; j++) e[j]=0.0; e[i]=1.0; LUsubst(n, a, p, e); for(j=0; j<n; j++) ib[j][i]=e[j]; } /* for i=1..n */ MatrixFree(n, a); VectorFree(n, e); IntVectorFree(n, p); } /* InversMatrix */
void CRagdollProp::SetupBones( matrix3x4_t *pBoneToWorld, int boneMask ) { studiohdr_t *pStudioHdr = GetModelPtr( ); bool sim[MAXSTUDIOBONES]; memset( sim, 0, pStudioHdr->numbones ); int i; for ( i = 0; i < m_ragdoll.listCount; i++ ) { if ( RagdollGetBoneMatrix( m_ragdoll, pBoneToWorld, i ) ) { sim[m_ragdoll.boneIndex[i]] = true; } } mstudiobone_t *pbones = pStudioHdr->pBone( 0 ); for ( i = 0; i < pStudioHdr->numbones; i++ ) { if ( sim[i] ) continue; MatrixCopy( pBoneToWorld[pbones[i].parent], pBoneToWorld[ i ] ); } }
void MatrixTranspose(const GzMatrix mat, GzMatrix result) { GzMatrix temp; for(int i=0; i<4; i++) for(int j=0; j<4; j++) temp[j][i] = mat[i][j]; MatrixCopy(temp, result); }
void GetAttachmentLocalSpace( CStudioHdr *pstudiohdr, int attachIndex, matrix3x4_t &pLocalToWorld ) { if ( attachIndex >= 0 ) { const mstudioattachment_t &pAttachment = pstudiohdr->pAttachment(attachIndex); MatrixCopy( pAttachment.local, pLocalToWorld ); } }
void MatrixMultiplyScale(matrix_t m, vec_t x, vec_t y, vec_t z) { matrix_t tmp, scale; MatrixCopy(m, tmp); MatrixSetupScale(scale, x, y, z); MatrixMultiply(scale, tmp, m); }
void MatrixMultiplyTranslation(matrix_t m, vec_t x, vec_t y, vec_t z) { matrix_t tmp, trans; MatrixCopy(m, tmp); MatrixSetupTranslation(trans, x, y, z); MatrixMultiply(trans, tmp, m); }
void MatrixMultiplyRotation(matrix_t m, vec_t pitch, vec_t yaw, vec_t roll) { matrix_t tmp, rot; MatrixCopy(m, tmp); MatrixFromAngles(rot, pitch, yaw, roll); MatrixMultiply(rot, tmp, m); }
/* *------------------------------------------------------ * Transpose a into result r *------------------------------------------------------ */ void MxTranspose( Matrix4 *a, Matrix4 *r) { register int i,j; Matrix4 tmp; for(i = 0; i < 4; i++) for(j = 0; j < 4; j++) tmp.element[j][i]=a->element[i][j]; MatrixCopy( &tmp, r); }
void CStickyBomb::Touch( CBaseEntity *pOther ) { // Don't stick if already stuck if ( GetMoveType() == MOVETYPE_FLYGRAVITY ) { trace_t tr = GetTouchTrace(); // stickies don't stick to each other or sky if ( FClassnameIs(pOther, "grenade_stickybomb") || (tr.surface.flags & SURF_SKY) ) { // bounce Vector vecNewVelocity; PhysicsClipVelocity( GetAbsVelocity(), tr.plane.normal, vecNewVelocity, 1.0 ); SetAbsVelocity( vecNewVelocity ); } else { SetAbsVelocity( vec3_origin ); SetMoveType( MOVETYPE_NONE ); if ( pOther->entindex() != 0 ) { // set up notification if the parent is deleted before we explode g_pNotify->AddEntity( this, pOther ); if ( (tr.surface.flags & SURF_HITBOX) && modelinfo->GetModelType( pOther->GetModel() ) == mod_studio ) { CBaseAnimating *pOtherAnim = dynamic_cast<CBaseAnimating *>(pOther); if ( pOtherAnim ) { matrix3x4_t bombWorldSpace; MatrixCopy( EntityToWorldTransform(), bombWorldSpace ); // get the bone info so we can follow the bone FollowEntity( pOther ); SetOwnerEntity( pOther ); m_boneIndexAttached = pOtherAnim->GetHitboxBone( tr.hitbox ); matrix3x4_t boneToWorld; pOtherAnim->GetBoneTransform( m_boneIndexAttached, boneToWorld ); // transform my current position/orientation into the hit bone's space // UNDONE: Eventually we need to intersect with the mesh here // REVISIT: maybe do something like the decal code to find a spot on // the mesh. matrix3x4_t worldToBone, localMatrix; MatrixInvert( boneToWorld, worldToBone ); ConcatTransforms( worldToBone, bombWorldSpace, localMatrix ); MatrixAngles( localMatrix, m_boneAngles.GetForModify(), m_bonePosition.GetForModify() ); return; } } SetParent( pOther ); } } } }
void ArrayAppendMatrix(array **tdst, matrix *msrc) { if((*tdst)->order > 0){ if((*tdst)->m[(*tdst)->order-1]->row != msrc->row){ fprintf(stderr, "Error while appending matrix to array! Object size differ: matrix: %u; array: %u", (unsigned int)msrc->row, (unsigned int)(*tdst)->m[(*tdst)->order-1]->row); abort(); } else{ (*tdst)->order += 1; (*tdst)->m = xrealloc((*tdst)->m, sizeof(matrix*)*(*tdst)->order); NewMatrix(&(*tdst)->m[(*tdst)->order-1], msrc->row, msrc->col); MatrixCopy(msrc, &(*tdst)->m[(*tdst)->order-1]); } } else{ (*tdst)->order = 1; (*tdst)->m = xmalloc(sizeof(matrix*)*1); NewMatrix(&(*tdst)->m[0], msrc->row, msrc->col); MatrixCopy(msrc, &(*tdst)->m[0]); } }
//----------------------------------------------------------------------------- // Render setup //----------------------------------------------------------------------------- bool CStaticProp::SetupBones( matrix3x4_t *pBoneToWorldOut, int nMaxBones, int boneMask, float currentTime ) { if (!m_pModel) return false; // Just copy it on down baby studiohdr_t *pStudioHdr = modelinfo->GetStudiomodel( m_pModel ); for (int i = 0; i < pStudioHdr->numbones; i++) { MatrixCopy( m_ModelToWorld, pBoneToWorldOut[i] ); } return true; }
//----------------------------------------------------------------------------- // Computes PoseToWorld from BoneToWorld //----------------------------------------------------------------------------- void ComputePoseToWorld( matrix3x4_t *pPoseToWorld, studiohdr_t *pStudioHdr, int boneMask, const Vector& vecViewOrigin, const matrix3x4_t *pBoneToWorld ) { if ( pStudioHdr->flags & STUDIOHDR_FLAGS_STATIC_PROP ) { // by definition, these always have an identity poseToBone transform MatrixCopy( pBoneToWorld[ 0 ], pPoseToWorld[ 0 ] ); return; } if ( !pStudioHdr->pLinearBones() ) { // convert bone to world transformations into pose to world transformations for (int i = 0; i < pStudioHdr->numbones; i++) { mstudiobone_t *pCurBone = pStudioHdr->pBone( i ); if ( !(pCurBone->flags & boneMask) ) continue; ConcatTransforms( pBoneToWorld[ i ], pCurBone->poseToBone, pPoseToWorld[ i ] ); } } else { mstudiolinearbone_t *pLinearBones = pStudioHdr->pLinearBones(); // convert bone to world transformations into pose to world transformations for (int i = 0; i < pStudioHdr->numbones; i++) { if ( !(pLinearBones->flags(i) & boneMask) ) continue; ConcatTransforms( pBoneToWorld[ i ], pLinearBones->poseToBone(i), pPoseToWorld[ i ] ); } } #if 0 // These don't seem to be used in any existing QC file, re-enable in a future project? // Pretransform if( !( pCurBone->flags & ( BONE_SCREEN_ALIGN_SPHERE | BONE_SCREEN_ALIGN_CYLINDER ))) { ConcatTransforms( pBoneToWorld[ i ], pCurBone->poseToBone, pPoseToWorld[ i ] ); } else { // If this bone is screen aligned, then generate a PoseToWorld matrix that billboards the bone ScreenAlignBone( &pPoseToWorld[i], pCurBone, vecViewOrigin, pBoneToWorld[i] ); } #endif }
void MatrixMultiplyScale(mat4_t m, vec_t x, vec_t y, vec_t z) { #if 0 mat4_t tmp, scale; MatrixCopy(m, tmp); MatrixSetupScale(scale, x, y, z); MatrixMultiplyMOD(tmp, scale, m); #else m[0] *= x; m[4] *= y; m[8] *= z; m[1] *= x; m[5] *= y; m[9] *= z; m[2] *= x; m[6] *= y; m[10] *= z; m[3] *= x; m[7] *= y; m[11] *= z; #endif }
void MatrixMultiply( const VMatrix& src1, const VMatrix& src2, VMatrix& dst ) { // Make sure it works if src1 == dst or src2 == dst VMatrix tmp1, tmp2; const VMatrix& s1 = (&src1 == &dst) ? tmp1 : src1; const VMatrix& s2 = (&src2 == &dst) ? tmp2 : src2; if (&src1 == &dst) { MatrixCopy( src1, tmp1 ); } if (&src2 == &dst) { MatrixCopy( src2, tmp2 ); } dst[0][0] = s1[0][0] * s2[0][0] + s1[0][1] * s2[1][0] + s1[0][2] * s2[2][0] + s1[0][3] * s2[3][0]; dst[0][1] = s1[0][0] * s2[0][1] + s1[0][1] * s2[1][1] + s1[0][2] * s2[2][1] + s1[0][3] * s2[3][1]; dst[0][2] = s1[0][0] * s2[0][2] + s1[0][1] * s2[1][2] + s1[0][2] * s2[2][2] + s1[0][3] * s2[3][2]; dst[0][3] = s1[0][0] * s2[0][3] + s1[0][1] * s2[1][3] + s1[0][2] * s2[2][3] + s1[0][3] * s2[3][3]; dst[1][0] = s1[1][0] * s2[0][0] + s1[1][1] * s2[1][0] + s1[1][2] * s2[2][0] + s1[1][3] * s2[3][0]; dst[1][1] = s1[1][0] * s2[0][1] + s1[1][1] * s2[1][1] + s1[1][2] * s2[2][1] + s1[1][3] * s2[3][1]; dst[1][2] = s1[1][0] * s2[0][2] + s1[1][1] * s2[1][2] + s1[1][2] * s2[2][2] + s1[1][3] * s2[3][2]; dst[1][3] = s1[1][0] * s2[0][3] + s1[1][1] * s2[1][3] + s1[1][2] * s2[2][3] + s1[1][3] * s2[3][3]; dst[2][0] = s1[2][0] * s2[0][0] + s1[2][1] * s2[1][0] + s1[2][2] * s2[2][0] + s1[2][3] * s2[3][0]; dst[2][1] = s1[2][0] * s2[0][1] + s1[2][1] * s2[1][1] + s1[2][2] * s2[2][1] + s1[2][3] * s2[3][1]; dst[2][2] = s1[2][0] * s2[0][2] + s1[2][1] * s2[1][2] + s1[2][2] * s2[2][2] + s1[2][3] * s2[3][2]; dst[2][3] = s1[2][0] * s2[0][3] + s1[2][1] * s2[1][3] + s1[2][2] * s2[2][3] + s1[2][3] * s2[3][3]; dst[3][0] = s1[3][0] * s2[0][0] + s1[3][1] * s2[1][0] + s1[3][2] * s2[2][0] + s1[3][3] * s2[3][0]; dst[3][1] = s1[3][0] * s2[0][1] + s1[3][1] * s2[1][1] + s1[3][2] * s2[2][1] + s1[3][3] * s2[3][1]; dst[3][2] = s1[3][0] * s2[0][2] + s1[3][1] * s2[1][2] + s1[3][2] * s2[2][2] + s1[3][3] * s2[3][2]; dst[3][3] = s1[3][0] * s2[0][3] + s1[3][1] * s2[1][3] + s1[3][2] * s2[2][3] + s1[3][3] * s2[3][3]; }
void MatrixAffineInverse(const mat4_t in, mat4_t out) { #if 0 MatrixCopy(in, out); MatrixInverse(out); #else // cleaned up out[0] = in[0]; out[4] = in[1]; out[8] = in[2]; out[1] = in[4]; out[5] = in[5]; out[9] = in[6]; out[2] = in[8]; out[6] = in[9]; out[10] = in[10]; out[3] = 0; out[7] = 0; out[11] = 0; out[15] = 1; out[12] = -(in[12] * out[0] + in[13] * out[4] + in[14] * out[8]); out[13] = -(in[12] * out[1] + in[13] * out[5] + in[14] * out[9]); out[14] = -(in[12] * out[2] + in[13] * out[6] + in[14] * out[10]); #endif }
//----------------------------------------------------------------------------- // Purpose: We need to slam our position! //----------------------------------------------------------------------------- void C_NPC_Puppet::BuildTransformations( CStudioHdr *pStudioHdr, Vector *pos, Quaternion q[], const matrix3x4_t& cameraTransform, int boneMask, CBoneBitList &boneComputed ) { if ( m_hAnimationTarget && m_nTargetAttachment != -1 ) { C_BaseAnimating *pTarget = m_hAnimationTarget->GetBaseAnimating(); if ( pTarget ) { matrix3x4_t matTarget; pTarget->GetAttachment( m_nTargetAttachment, matTarget ); MatrixCopy( matTarget, GetBoneForWrite( 0 ) ); boneComputed.ClearAll(); // FIXME: Why is this calculated already? boneComputed.MarkBone( 0 ); } } // Call the baseclass BaseClass::BuildTransformations( pStudioHdr, pos, q, cameraTransform, boneMask, boneComputed ); }
static int register_mri(MRI *mri_in, MRI *mri_ref, MORPH_PARMS *parms, MATRIX *m_L) { MRI *mri_in_windowed, *mri_ref_windowed ; fprintf(stderr, "aligning volume with average...\n") ; if (window_size > 0) { double in_means[3], ref_means[3] ; MRIcenterOfMass(mri_in, in_means, 0) ; MRIcenterOfMass(mri_ref, ref_means, 0) ; printf("windowing ref around (%d, %d, %d) and input around (%d, %d, %d)\n", nint(ref_means[0]), nint(ref_means[1]), nint(ref_means[2]), nint(in_means[0]), nint(in_means[1]), nint(in_means[2])) ; mri_in_windowed = MRIwindow(mri_in, NULL, WINDOW_HANNING,nint(in_means[0]), nint(in_means[1]), nint(in_means[2]),window_size); mri_ref_windowed = MRIwindow(mri_ref,NULL,WINDOW_HANNING,nint(ref_means[0]), nint(ref_means[1]), nint(ref_means[2]),window_size); mri_in = mri_in_windowed ; mri_ref = mri_ref_windowed ; } MRIrigidAlign(mri_in, mri_ref, parms, m_L) ; fprintf(stderr, "final transform:\n") ; MatrixPrint(stderr, parms->lta->xforms[0].m_L) ; fprintf(stderr, "\n") ; if (Gdiag & DIAG_WRITE && DIAG_VERBOSE_ON) { MRI *mri_aligned ; mri_aligned = MRIapplyRASlinearTransform(mri_in, NULL, parms->lta->xforms[0].m_L) ; MRIwriteImageViews(mri_aligned, "after_alignment", IMAGE_SIZE) ; MRIfree(&mri_aligned) ; } MatrixCopy(parms->lta->xforms[0].m_L, m_L) ; return(NO_ERROR) ; }
void MatrixTest::TestMatrixSVDPseudoInverse() { float tolerance = 1e-5; std::cout << "\rMatrixTest::TestMatrixSVDPseudoInverse()\n"; // check the low-level routine first MATRIX *Ux = MatrixRead( (char*) ( SVD_U_MATRIX.c_str() ) ); MATRIX *Vx = MatrixRead( (char*) ( SVD_V_MATRIX.c_str() ) ); VECTOR *Sx = MatrixRead( (char*) ( SVD_S_VECTOR.c_str() ) ); CPPUNIT_ASSERT (Ux != NULL); CPPUNIT_ASSERT (Vx != NULL); CPPUNIT_ASSERT (Sx != NULL); MATRIX *U=MatrixCopy(mSquareMatrix,NULL); MATRIX *V=MatrixAlloc(U->cols, U->cols, MATRIX_REAL) ; VECTOR *S=VectorAlloc(U->cols, MATRIX_REAL) ; OpenSvdcmp( U, S, V ) ; CPPUNIT_ASSERT (U != NULL); CPPUNIT_ASSERT (V != NULL); CPPUNIT_ASSERT (S != NULL); CPPUNIT_ASSERT ( AreMatricesEqual( U, Ux, tolerance ) ); CPPUNIT_ASSERT ( AreMatricesEqual( V, Vx, tolerance ) ); CPPUNIT_ASSERT ( AreMatricesEqual( S, Sx, tolerance ) ); // now check MatrixSVDPseudoInverse, which uses sc_linalg_SV_decomp tolerance = 1e-5; MATRIX *actualInverse = MatrixSVDPseudoInverse( mNonSquareMatrix, NULL ); MATRIX *expectedInverse = MatrixRead( (char*) ( NON_SQUARE_MATRIX_PSEUDO_INVERSE.c_str() ) ); CPPUNIT_ASSERT( AreMatricesEqual( actualInverse, expectedInverse ) ); actualInverse = MatrixSVDPseudoInverse( mSquareMatrix, NULL ); expectedInverse = MatrixRead( (char*) ( SQUARE_MATRIX_PSEUDO_INVERSE.c_str() ) ); CPPUNIT_ASSERT ( AreMatricesEqual( actualInverse, expectedInverse, tolerance ) ); }
static void RagdollAddSolids( IPhysicsEnvironment *pPhysEnv, ragdoll_t &ragdoll, const ragdollparams_t ¶ms, cache_ragdollsolid_t *pSolids, int solidCount, const cache_ragdollconstraint_t *pConstraints, int constraintCount ) { const char *pszName = params.pStudioHdr->pszName(); Vector position; matrix3x4_t xform; // init parent index for ( int i = 0; i < solidCount; i++ ) { ragdoll.list[i].parentIndex = -1; } // now set from constraints for ( int i = 0; i < constraintCount; i++ ) { // save parent index ragdoll.list[pConstraints[i].childIndex].parentIndex = pConstraints[i].parentIndex; MatrixGetColumn( pConstraints[i].constraintToAttached, 3, ragdoll.list[pConstraints[i].childIndex].originParentSpace ); } // now setup the solids, using parent indices for ( int i = 0; i < solidCount; i++ ) { ragdoll.boneIndex[i] = pSolids[i].boneIndex; pSolids[i].params.pName = pszName; pSolids[i].params.pGameData = params.pGameData; ragdoll.list[i].pObject = pPhysEnv->CreatePolyObject( params.pCollide->solids[pSolids[i].collideIndex], pSolids[i].surfacePropIndex, vec3_origin, vec3_angle, &pSolids[i].params ); ragdoll.list[i].pObject->SetGameIndex( i ); int parentIndex = ragdoll.list[i].parentIndex; MatrixCopy( params.pCurrentBones[ragdoll.boneIndex[i]], xform ); if ( parentIndex >= 0 ) { Assert(parentIndex<i); ragdoll.list[parentIndex].pObject->LocalToWorld( &position, ragdoll.list[i].originParentSpace ); MatrixSetColumn( position, 3, xform ); } ragdoll.list[i].pObject->SetPositionMatrix( xform, true ); PhysSetGameFlags( ragdoll.list[i].pObject, FVPHYSICS_PART_OF_RAGDOLL ); } ragdoll.listCount = solidCount; }
static int order_eigenvectors(MATRIX *m_src_evectors, MATRIX *m_dst_evectors) { int row, col, xcol, ycol, zcol ; double mx ; if (m_src_evectors == m_dst_evectors) m_src_evectors = MatrixCopy(m_src_evectors, NULL) ; /* find columx with smallest dot product with unit x vector */ mx = fabs(*MATRIX_RELT(m_src_evectors, 1, 1)) ; xcol = 1 ; for (col = 2 ; col <= 3 ; col++) if (fabs(*MATRIX_RELT(m_src_evectors, 1, col)) > mx) { xcol = col ; mx = fabs(*MATRIX_RELT(m_src_evectors, 1, col)) ; } mx = fabs(*MATRIX_RELT(m_src_evectors, 2, 1)) ; ycol = 1 ; for (col = 2 ; col <= 3 ; col++) if (*MATRIX_RELT(m_src_evectors, 2, col) > mx) { ycol = col ; mx = fabs(*MATRIX_RELT(m_src_evectors, 2, col)) ; } mx = fabs(*MATRIX_RELT(m_src_evectors, 3, 1)) ; zcol = 1 ; for (col = 2 ; col <= 3 ; col++) if (fabs(*MATRIX_RELT(m_src_evectors, 3, col)) > mx) { zcol = col ; mx = fabs(*MATRIX_RELT(m_src_evectors, 3, col)) ; } for (row = 1 ; row <= 3 ; row++) { *MATRIX_RELT(m_dst_evectors,row,1) = *MATRIX_RELT(m_src_evectors,row,xcol); *MATRIX_RELT(m_dst_evectors,row,2) = *MATRIX_RELT(m_src_evectors,row,ycol); *MATRIX_RELT(m_dst_evectors,row,3) = *MATRIX_RELT(m_src_evectors,row,zcol); } return(NO_ERROR) ; }
/* last column must be an integer column which define the classes */ void LDA(matrix *mx, uivector *y, LDAMODEL *lda) { size_t i, j, l, k, cc, imin, imax; array *classes; array *S; matrix *X, *X_T, *Sb, *Sw, *InvSw_Sb; dvector *mutot; dvector *classmu; dvector *evect_, *ldfeature; matrix *covmx; lda->nclass = 0; imin = imax = y->data[0]; for(i = 1; i < y->size; i++){ if(y->data[i] > imax){ imax = y->data[i]; } if(y->data[i] < imin){ imin = y->data[i]; } } /* get the number of classes */ if(imin == 0){ lda->class_start = 0; lda->nclass = imax + 1; } else{ lda->class_start = 1; lda->nclass = imax; } /*printf("nclass %d\n", (int)lda->nclass);*/ /* Copy data */ NewMatrix(&X, mx->row, mx->col); MatrixCopy(mx, &X); MatrixCheck(X); /* for(j = 0; j < mx->col-1; j++){ for(i = 0; i < mx->row; i++){ X->data[i][j] = mx->data[i][j]; } } */ /*create classes of objects */ NewArray(&classes, lda->nclass); UIVectorResize(&lda->classid, mx->row); j = 0; if(imin == 0){ for(k = 0; k < lda->nclass; k++){ cc = 0; for(i = 0; i < X->row; i++){ if(y->data[i] == k) cc++; else continue; } NewArrayMatrix(&classes, k, cc, X->col); cc = 0; for(i = 0; i < X->row; i++){ if(y->data[i] == k){ for(l = 0; l < X->col; l++){ classes->m[k]->data[cc][l] = X->data[i][l]; } lda->classid->data[j] = i; j++; cc++; } else{ continue; } } } } else{ for(k = 0; k < lda->nclass; k++){ cc = 0; for(i = 0; i < X->row; i++){ if(y->data[i] == k+1) cc++; else continue; } NewArrayMatrix(&classes, k, cc, X->col); cc = 0; for(i = 0; i < X->row; i++){ if(y->data[i] == k+1){ for(l = 0; l < X->col; l++){ classes->m[k]->data[cc][l] = X->data[i][l]; } lda->classid->data[j] = i; j++; cc++; } else{ continue; } } } } /*puts("Classes"); PrintArray(classes);*/ /* Compute the prior probability */ for(k = 0; k < classes->order; k++){ DVectorAppend(&lda->pprob, (classes->m[k]->row/(double)X->row)); } /* puts("Prior Probability"); PrintDVector(lda->pprob); */ /*Compute the mean of each class*/ for(k = 0; k < classes->order; k++){ initDVector(&classmu); MatrixColAverage(classes->m[k], &classmu); MatrixAppendRow(&lda->mu, classmu); DelDVector(&classmu); } /*puts("Class Mu"); FindNan(lda->mu); PrintMatrix(lda->mu);*/ /*Calculate the total mean of samples..*/ initDVector(&mutot); MatrixColAverage(X, &mutot); /*puts("Mu tot"); PrintDVector(mutot);*/ /*NewDVector(&mutot, mu->col); for(k = 0; k < mu->row; k++){ for(i = 0; i < mu->col; i++){ mutot->data[i] += mu->data[k][i]; } } for(i = 0; i < mutot->size; i++){ mutot->data[i] /= nclasses; }*/ /*Centering data before computing the scatter matrix*/ for(k = 0; k < classes->order; k++){ for(i = 0; i < classes->m[k]->row; i++){ for(j = 0; j < classes->m[k]->col; j++){ classes->m[k]->data[i][j] -= mutot->data[j]; } } } /* puts("Classes"); for(i = 0; i < classes->order; i++){ FindNan(classes->m[i]); } PrintArray(classes); */ /*Compute the scatter matrix * S = nobj - 1 * covmx */ initArray(&S); NewMatrix(&covmx, X->col, X->col); for(k = 0; k < classes->order; k++){ matrix *m_T; NewMatrix(&m_T, classes->m[k]->col, classes->m[k]->row); MatrixTranspose(classes->m[k], m_T); MatrixDotProduct(m_T, classes->m[k], covmx); for(i = 0; i < covmx->row; i++){ for(j = 0; j < covmx->col; j++){ covmx->data[i][j] /= classes->m[k]->row; } } ArrayAppendMatrix(&S, covmx); MatrixSet(covmx, 0.f); DelMatrix(&m_T); } /* puts("Scatter Matrix"); for(i = 0; i < S->order; i++) FindNan(S->m[i]); PrintArray(S);*/ /* Compute the class scatter which represent the covariance matrix */ NewMatrix(&Sw, X->col, X->col); for(k = 0; k < S->order; k++){ for(i = 0; i < S->m[k]->row; i++){ for(j = 0; j < S->m[k]->col; j++){ Sw->data[i][j] += (double)(classes->m[k]->row/(double)X->row) * S->m[k]->data[i][j]; } } } /* puts("Class scatter matrix Sw"); FindNan(Sw); PrintMatrix(Sw); */ /*Compute the between class scatter matrix Sb*/ NewMatrix(&Sb, X->col, X->col); for(k = 0; k < lda->mu->row; k++){ /*for each class of object*/ cc = classes->m[k]->row; for(i = 0; i < Sb->row; i++){ for(j = 0; j < Sb->col; j++){ Sb->data[i][j] += cc * (lda->mu->data[k][i] - mutot->data[i]) * (lda->mu->data[k][j] - mutot->data[j]); } } } /* puts("Between class scatter matrix Sb"); FindNan(Sb); PrintMatrix(Sb); */ /* Computing the LDA projection */ /*puts("Compute Matrix Inversion");*/ MatrixInversion(Sw, &lda->inv_cov); double ss = 0.f; for(i = 0; i < lda->inv_cov->row; i++){ for(j = 0; j < lda->inv_cov->col; j++){ ss += square(lda->inv_cov->data[i][j]); } if(_isnan_(ss)) break; } if(FLOAT_EQ(ss, 0.f, EPSILON) || _isnan_(ss)){ /*Do SVD as pseudoinversion accordin to Liu et al. because matrix is nonsingular * * JUN LIU et al, Int. J. Patt. Recogn. Artif. Intell. 21, 1265 (2007). DOI: 10.1142/S0218001407005946 * EFFICIENT PSEUDOINVERSE LINEAR DISCRIMINANT ANALYSIS AND ITS NONLINEAR FORM FOR FACE RECOGNITION * * * Sw`^-1 = Q * G^-1 * Q_T * Q G AND Q_T come from SVD */ MatrixPseudoinversion(Sw, &lda->inv_cov); /* NewMatrix(&A_T, Sw->col, Sw->row); MatrixInversion(Sw, &A_T); NewMatrix(&A_T_Sw, A_T->row, Sw->col); MatrixDotProduct(A_T, Sw, A_T_Sw); initMatrix(&A_T_Sw_inv); MatrixInversion(A_T_Sw, &A_T_Sw_inv); MatrixDotProduct(A_T_Sw_inv, A_T, lda->inv_cov); DelMatrix(&A_T); DelMatrix(&A_T_Sw); DelMatrix(&A_T_Sw_inv); */ } /*puts("Inverted Covariance Matrix from Sw"); FindNan(lda->inv_cov); PrintMatrix(lda->inv_cov); */ /*puts("Compute Matrix Dot Product");*/ NewMatrix(&InvSw_Sb, lda->inv_cov->row, Sb->col); MatrixDotProduct(lda->inv_cov, Sb, InvSw_Sb); /*puts("InvSw_Sb"); PrintMatrix(InvSw_Sb);*/ /*puts("Compute Eigenvectors");*/ EVectEval(InvSw_Sb, &lda->eval, &lda->evect); /*EvectEval3(InvSw_Sb, InvSw_Sb->row, &lda->eval, &lda->evect);*/ /*EVectEval(InvSw_Sb, &lda->eval, &lda->evect); */ /* Calculate the new projection in the feature space * * and the multivariate normal distribution */ /* Remove centering data */ for(k = 0; k < classes->order; k++){ for(i = 0; i < classes->m[k]->row; i++){ for(j = 0; j < classes->m[k]->col; j++){ classes->m[k]->data[i][j] += mutot->data[j]; } } } initMatrix(&X_T); for(k = 0; k < classes->order; k++){ /*printf("row %d col %d\n", (int)classes->m[k]->row, (int)classes->m[k]->col);*/ AddArrayMatrix(&lda->features, classes->m[k]->row, classes->m[k]->col); AddArrayMatrix(&lda->mnpdf, classes->m[k]->row, classes->m[k]->col); } NewDVector(&evect_, lda->evect->row); initDVector(&ldfeature); ResizeMatrix(&lda->fmean, classes->order, lda->evect->col); ResizeMatrix(&lda->fsdev, classes->order, lda->evect->col); for(l = 0; l < lda->evect->col; l++){ for(i = 0; i < lda->evect->row; i++){ evect_->data[i] = lda->evect->data[i][l]; } for(k = 0; k < classes->order; k++){ ResizeMatrix(&X_T, classes->m[k]->col, classes->m[k]->row); MatrixTranspose(classes->m[k], X_T); DVectorResize(&ldfeature, classes->m[k]->row); DVectorMatrixDotProduct(X_T, evect_, ldfeature); for(i = 0; i < ldfeature->size; i++){ lda->features->m[k]->data[i][l] = ldfeature->data[i]; } /* Calculate the multivariate normal distribution */ double mean = 0.f, sdev = 0.f; DVectorMean(ldfeature, &mean); DVectorSDEV(ldfeature, &sdev); lda->fmean->data[k][l] = mean; lda->fsdev->data[k][l] = sdev; for(i = 0; i < ldfeature->size; i++){ lda->mnpdf->m[k]->data[i][l] = 1./sqrt(2 * pi* sdev) * exp(-square((ldfeature->data[i] - mean)/sdev)/2.f); } } } DelDVector(&evect_); DelMatrix(&covmx); DelDVector(&ldfeature); DelDVector(&mutot); DelMatrix(&Sb); DelMatrix(&InvSw_Sb); DelArray(&classes); DelArray(&S); DelMatrix(&Sw); DelMatrix(&X_T); DelMatrix(&X); }
/* ==================== StudioMergeBones ==================== */ void CStudioModelRenderer::StudioMergeBones ( model_t *m_pSubModel ) { int i, j; double f; int do_hunt = true; mstudiobone_t *pbones; mstudioseqdesc_t *pseqdesc; mstudioanim_t *panim; static float pos[MAXSTUDIOBONES][3]; float bonematrix[3][4]; static vec4_t q[MAXSTUDIOBONES]; if (m_pCurrentEntity->curstate.sequence >= m_pStudioHeader->numseq) { m_pCurrentEntity->curstate.sequence = 0; } pseqdesc = (mstudioseqdesc_t *)((byte *)m_pStudioHeader + m_pStudioHeader->seqindex) + m_pCurrentEntity->curstate.sequence; f = StudioEstimateFrame( pseqdesc ); if (m_pCurrentEntity->latched.prevframe > f) { //Con_DPrintf("%f %f\n", m_pCurrentEntity->prevframe, f ); } panim = StudioGetAnim( m_pSubModel, pseqdesc ); StudioCalcRotations( pos, q, pseqdesc, panim, f ); pbones = (mstudiobone_t *)((byte *)m_pStudioHeader + m_pStudioHeader->boneindex); for (i = 0; i < m_pStudioHeader->numbones; i++) { for (j = 0; j < m_nCachedBones; j++) { if (stricmp(pbones[i].name, m_nCachedBoneNames[j]) == 0) { MatrixCopy( m_rgCachedBoneTransform[j], (*m_pbonetransform)[i] ); MatrixCopy( m_rgCachedLightTransform[j], (*m_plighttransform)[i] ); break; } } if (j >= m_nCachedBones) { QuaternionMatrix( q[i], bonematrix ); bonematrix[0][3] = pos[i][0]; bonematrix[1][3] = pos[i][1]; bonematrix[2][3] = pos[i][2]; if (pbones[i].parent == -1) { if ( IEngineStudio.IsHardware() ) { ConcatTransforms ((*m_protationmatrix), bonematrix, (*m_pbonetransform)[i]); // MatrixCopy should be faster... //ConcatTransforms ((*m_protationmatrix), bonematrix, (*m_plighttransform)[i]); MatrixCopy( (*m_pbonetransform)[i], (*m_plighttransform)[i] ); } else { ConcatTransforms ((*m_paliastransform), bonematrix, (*m_pbonetransform)[i]); ConcatTransforms ((*m_protationmatrix), bonematrix, (*m_plighttransform)[i]); } // Apply client-side effects to the transformation matrix StudioFxTransform( m_pCurrentEntity, (*m_pbonetransform)[i] ); } else { ConcatTransforms ((*m_pbonetransform)[pbones[i].parent], bonematrix, (*m_pbonetransform)[i]); ConcatTransforms ((*m_plighttransform)[pbones[i].parent], bonematrix, (*m_plighttransform)[i]); } } } }
/* ==================== StudioSetupBones ==================== */ void CStudioModelRenderer::StudioSetupBones ( void ) { int i; double f; mstudiobone_t *pbones; mstudioseqdesc_t *pseqdesc; mstudioanim_t *panim; static float pos[MAXSTUDIOBONES][3]; static vec4_t q[MAXSTUDIOBONES]; float bonematrix[3][4]; static float pos2[MAXSTUDIOBONES][3]; static vec4_t q2[MAXSTUDIOBONES]; static float pos3[MAXSTUDIOBONES][3]; static vec4_t q3[MAXSTUDIOBONES]; static float pos4[MAXSTUDIOBONES][3]; static vec4_t q4[MAXSTUDIOBONES]; if (m_pCurrentEntity->curstate.sequence >= m_pStudioHeader->numseq) { m_pCurrentEntity->curstate.sequence = 0; } pseqdesc = (mstudioseqdesc_t *)((byte *)m_pStudioHeader + m_pStudioHeader->seqindex) + m_pCurrentEntity->curstate.sequence; f = StudioEstimateFrame( pseqdesc ); if (m_pCurrentEntity->latched.prevframe > f) { //Con_DPrintf("%f %f\n", m_pCurrentEntity->prevframe, f ); } panim = StudioGetAnim( m_pRenderModel, pseqdesc ); StudioCalcRotations( pos, q, pseqdesc, panim, f ); if (pseqdesc->numblends > 1) { float s; float dadt; panim += m_pStudioHeader->numbones; StudioCalcRotations( pos2, q2, pseqdesc, panim, f ); dadt = StudioEstimateInterpolant(); s = (m_pCurrentEntity->curstate.blending[0] * dadt + m_pCurrentEntity->latched.prevblending[0] * (1.0 - dadt)) / 255.0; StudioSlerpBones( q, pos, q2, pos2, s ); if (pseqdesc->numblends == 4) { panim += m_pStudioHeader->numbones; StudioCalcRotations( pos3, q3, pseqdesc, panim, f ); panim += m_pStudioHeader->numbones; StudioCalcRotations( pos4, q4, pseqdesc, panim, f ); s = (m_pCurrentEntity->curstate.blending[0] * dadt + m_pCurrentEntity->latched.prevblending[0] * (1.0 - dadt)) / 255.0; StudioSlerpBones( q3, pos3, q4, pos4, s ); s = (m_pCurrentEntity->curstate.blending[1] * dadt + m_pCurrentEntity->latched.prevblending[1] * (1.0 - dadt)) / 255.0; StudioSlerpBones( q, pos, q3, pos3, s ); } } if (m_fDoInterp && m_pCurrentEntity->latched.sequencetime && ( m_pCurrentEntity->latched.sequencetime + 0.2 > m_clTime ) && ( m_pCurrentEntity->latched.prevsequence < m_pStudioHeader->numseq )) { // blend from last sequence static float pos1b[MAXSTUDIOBONES][3]; static vec4_t q1b[MAXSTUDIOBONES]; float s; pseqdesc = (mstudioseqdesc_t *)((byte *)m_pStudioHeader + m_pStudioHeader->seqindex) + m_pCurrentEntity->latched.prevsequence; panim = StudioGetAnim( m_pRenderModel, pseqdesc ); // clip prevframe StudioCalcRotations( pos1b, q1b, pseqdesc, panim, m_pCurrentEntity->latched.prevframe ); if (pseqdesc->numblends > 1) { panim += m_pStudioHeader->numbones; StudioCalcRotations( pos2, q2, pseqdesc, panim, m_pCurrentEntity->latched.prevframe ); s = (m_pCurrentEntity->latched.prevseqblending[0]) / 255.0; StudioSlerpBones( q1b, pos1b, q2, pos2, s ); if (pseqdesc->numblends == 4) { panim += m_pStudioHeader->numbones; StudioCalcRotations( pos3, q3, pseqdesc, panim, m_pCurrentEntity->latched.prevframe ); panim += m_pStudioHeader->numbones; StudioCalcRotations( pos4, q4, pseqdesc, panim, m_pCurrentEntity->latched.prevframe ); s = (m_pCurrentEntity->latched.prevseqblending[0]) / 255.0; StudioSlerpBones( q3, pos3, q4, pos4, s ); s = (m_pCurrentEntity->latched.prevseqblending[1]) / 255.0; StudioSlerpBones( q1b, pos1b, q3, pos3, s ); } } s = 1.0 - (m_clTime - m_pCurrentEntity->latched.sequencetime) / 0.2; StudioSlerpBones( q, pos, q1b, pos1b, s ); } else { //Con_DPrintf("prevframe = %4.2f\n", f); m_pCurrentEntity->latched.prevframe = f; } pbones = (mstudiobone_t *)((byte *)m_pStudioHeader + m_pStudioHeader->boneindex); // calc gait animation if (m_pPlayerInfo && m_pPlayerInfo->gaitsequence != 0) { if (m_pPlayerInfo->gaitsequence >= m_pStudioHeader->numseq) { m_pPlayerInfo->gaitsequence = 0; } pseqdesc = (mstudioseqdesc_t *)((byte *)m_pStudioHeader + m_pStudioHeader->seqindex) + m_pPlayerInfo->gaitsequence; panim = StudioGetAnim( m_pRenderModel, pseqdesc ); StudioCalcRotations( pos2, q2, pseqdesc, panim, m_pPlayerInfo->gaitframe ); for (i = 0; i < m_pStudioHeader->numbones; i++) { if (strcmp( pbones[i].name, "Bip01 Spine") == 0) break; memcpy( pos[i], pos2[i], sizeof( pos[i] )); memcpy( q[i], q2[i], sizeof( q[i] )); } } for (i = 0; i < m_pStudioHeader->numbones; i++) { QuaternionMatrix( q[i], bonematrix ); bonematrix[0][3] = pos[i][0]; bonematrix[1][3] = pos[i][1]; bonematrix[2][3] = pos[i][2]; if (pbones[i].parent == -1) { if ( IEngineStudio.IsHardware() ) { ConcatTransforms ((*m_protationmatrix), bonematrix, (*m_pbonetransform)[i]); // MatrixCopy should be faster... //ConcatTransforms ((*m_protationmatrix), bonematrix, (*m_plighttransform)[i]); MatrixCopy( (*m_pbonetransform)[i], (*m_plighttransform)[i] ); } else { ConcatTransforms ((*m_paliastransform), bonematrix, (*m_pbonetransform)[i]); ConcatTransforms ((*m_protationmatrix), bonematrix, (*m_plighttransform)[i]); } // Apply client-side effects to the transformation matrix StudioFxTransform( m_pCurrentEntity, (*m_pbonetransform)[i] ); } else { ConcatTransforms ((*m_pbonetransform)[pbones[i].parent], bonematrix, (*m_pbonetransform)[i]); ConcatTransforms ((*m_plighttransform)[pbones[i].parent], bonematrix, (*m_plighttransform)[i]); } } }
int main(int argc, CHAR *argv[]) { INT i; UINT begin; UINT end; UINT lapsed; MATRIX vtrans, Vinv; /* View transformation and inverse. */ /* * First, process command line arguments. */ i = 1; while ((i < argc) && (argv[i][0] == '-')) { switch (argv[i][1]) { case '?': case 'h': case 'H': Usage(); exit(1); case 'a': case 'A': AntiAlias = TRUE; if (argv[i][2] != '\0') { NumSubRays = atoi(&argv[i][2]); } else { NumSubRays = atoi(&argv[++i][0]); } break; case 'm': if (argv[i][2] != '\0') { MaxGlobMem = atoi(&argv[i][2]); } else { MaxGlobMem = atoi(&argv[++i][0]); } break; case 'p': if (argv[i][2] != '\0') { nprocs = atoi(&argv[i][2]); } else { nprocs = atoi(&argv[++i][0]); } break; case 's': case 'S': dostats = TRUE; break; default: fprintf(stderr, "%s: Invalid option \'%c\'.\n", ProgName, argv[i][0]); exit(1); } i++; } if (i == argc) { Usage(); exit(1); } /* * Make sure nprocs is within valid range. */ if (nprocs < 1 || nprocs > MAX_PROCS) { fprintf(stderr, "%s: Valid range for #processors is [1, %d].\n", ProgName, MAX_PROCS); exit(1); } /* * Print command line parameters. */ printf("\n"); printf("Number of processors: \t%ld\n", nprocs); printf("Global shared memory size:\t%ld MB\n", MaxGlobMem); printf("Samples per pixel: \t%ld\n", NumSubRays); printf("\n"); /* * Initialize the shared memory environment and request the total * amount of amount of shared memory we might need. This * includes memory for the database, grid, and framebuffer. */ MaxGlobMem <<= 20; /* Convert MB to bytes. */ MAIN_INITENV(,MaxGlobMem + 512*1024) THREAD_INIT_FREE(); gm = (GMEM *)G_MALLOC(sizeof(GMEM)); /* * Perform shared environment initializations. */ gm->nprocs = nprocs; gm->pid = 0; gm->rid = 1; BARINIT(gm->start, nprocs) LOCKINIT(gm->pidlock) LOCKINIT(gm->ridlock) LOCKINIT(gm->memlock) ALOCKINIT(gm->wplock, nprocs) /* POSSIBLE ENHANCEMENT: Here is where one might distribute the raystruct data structure across physically distributed memories as desired. */ if (!GlobalHeapInit(MaxGlobMem)) { fprintf(stderr, "%s: Cannot initialize global heap.\n", ProgName); exit(1); } /* * Initialize HUG parameters, read environment and geometry files. */ Huniform_defaults(); ReadEnvFile(/* *argv*/argv[i]); ReadGeoFile(GeoFileName); OpenFrameBuffer(); /* * Compute view transform and its inverse. */ CreateViewMatrix(); MatrixCopy(vtrans, View.vtrans); MatrixInverse(Vinv, vtrans); MatrixCopy(View.vtransInv, Vinv); /* * Print out what we have so far. */ printf("Number of primitive objects: \t%ld\n", prim_obj_cnt); printf("Number of primitive elements:\t%ld\n", prim_elem_cnt); /* * Preprocess database into hierarchical uniform grid. */ if (TraversalType == TT_HUG) BuildHierarchy_Uniform(); /* * Now create slave processes. */ CLOCK(begin) CREATE(StartRayTrace, gm->nprocs); WAIT_FOR_END(gm->nprocs); CLOCK(end) /* * We are finished. Clean up, print statistics and run time. */ CloseFrameBuffer(PicFileName); PrintStatistics(); lapsed = (end - begin) & 0x7FFFFFFF; printf("TIMING STATISTICS MEASURED BY MAIN PROCESS:\n"); printf(" Overall start time %20lu\n", begin); printf(" Overall end time %20lu\n", end); printf(" Total time with initialization %20lu\n", lapsed); printf(" Total time without initialization %20lu\n", end - gm->par_start_time); if (dostats) { unsigned totalproctime, maxproctime, minproctime; printf("\n\n\nPER-PROCESS STATISTICS:\n"); printf("%20s%20s\n","Proc","Time"); printf("%20s%20s\n\n","","Tracing Rays"); for (i = 0; i < gm->nprocs; i++) printf("%20ld%20ld\n",i,gm->partime[i]); totalproctime = gm->partime[0]; minproctime = gm->partime[0]; maxproctime = gm->partime[0]; for (i = 1; i < gm->nprocs; i++) { totalproctime += gm->partime[i]; if (gm->partime[i] > maxproctime) maxproctime = gm->partime[i]; if (gm->partime[i] < minproctime) minproctime = gm->partime[i]; } printf("\n\n%20s%20d\n","Max = ",maxproctime); printf("%20s%20d\n","Min = ",minproctime); printf("%20s%20d\n","Avg = ",(int) (((double) totalproctime) / ((double) (1.0 * gm->nprocs)))); } MAIN_END }