// Kalman Filter Prediction Step // We "predict" what the next data will be // What this means is the filter's mean will change // but we loose "certainty" about where the data is // (aka. the covariance will increase in this step) void KalmanPredict(struct KalmanFilter *kal) { struct Matrix x_next, P_next, F_trans; CreateBlankMatrix(x_next); CreateBlankMatrix(P_next); CreateBlankMatrix(F_trans); // Calculate x_next (update guassian mean) MatrixMult(&x_next, kal->updateMatrix, kal->meanVector); MatrixAdd(&x_next, x_next, kal->moveVector); // Calculate p_next (update guassian covariance); MatrixMult(&P_next, kal->updateMatrix, kal->covarianceMatrixX); MatrixTranspose(&F_trans, kal->updateMatrix); MatrixMult(&P_next, P_next, F_trans); // Copy results to the kalmanfilter class CopyMatrixByValue(&kal->meanVector, x_next); CopyMatrixByValue(&kal->covarianceMatrixX, P_next); DeleteMatrix(&x_next); DeleteMatrix(&P_next); DeleteMatrix(&F_trans); }
void mlrVoiceManager::Exec(Tpoint *campos, Trotation *camrot, Tpoint *camvel){ static const Tpoint upv = { 0, 0, 1 }; static const Tpoint fwd = { 1, 0, 0 }; MatrixMult(camrot, &upv, &listenerUp); MatrixMult(camrot, &fwd, &listenerFront); listenerPosition = *campos; listenerVelocity = *camvel; Lock(); mlrVoice *n; int channels; channels=g_nDynamicVoices; // sort the PlayList, store in temp AList temp; while (n=(mlrVoice *)PlayList.RemHead()){ n->PreExec(); temp.AddSorted((ANode *)n); } // filter out what will be played while (n=(mlrVoice *)temp.RemHead()){ if(( n->status==mlrVoice::VSSTART || n->status==mlrVoice::VSPLAYING ) && channels > 0){ PlayList.AddTail(n); // maintain sortedness channels--; } else { n->ReleaseBuffers(); HoldList.AddTail(n); } } channels=g_nDynamicVoices; // make these allocate their sound buffers // then exec() if (SimDriver.MotionOn()){ n=(mlrVoice *)PlayList.GetHead(); while (n){ //COUNT_PROFILE("VOICEMAN_COUNT"); n->AllocateBuffers(); n->Exec(); n=(mlrVoice *)n->GetSucc(); } } else{ n=(mlrVoice *)PlayList.GetHead(); while (n){ n->Pause(); n=(mlrVoice *)n->GetSucc(); } } Unlock(); }
rmatrix RSBase3D::GetMatrix() { if(LastFrame!=RSFrameCount) { matm=MatrixMult(MatrixMult(RotateYMatrix(RoleFactor), ScaleMatrixXYZ(ScaleFactorX,ScaleFactorY,ScaleFactorZ)),mat); if(parent)matm=MatrixMult(matm,parent->GetMatrix()); LastFrame=RSFrameCount; } return matm; }
// Kalman Update Step // We "update" given what we get for data // The filter will use the data given to lower our // uncertainty (aka. covariance) void KalmanUpdate(struct KalmanFilter *kal, struct Matrix meas) { struct Matrix y, S, extTran, K, Sinv, x_next, P_next; CreateBlankMatrix(y); CreateBlankMatrix(S); CreateBlankMatrix(extTran); CreateBlankMatrix(K); CreateBlankMatrix(Sinv); CreateBlankMatrix(x_next); CreateBlankMatrix(P_next); CopyMatrix(&kal->measurementVector, meas); // Find the difference between the move (measurement) // and what we predicted (extraction * data) MatrixMult(&y, kal->extractionMatrix, kal->meanVector); MatrixSub(&y, kal->measurementVector, y); // The Covariance of the move MatrixMult(&S, kal->extractionMatrix, kal->covarianceMatrixX); MatrixTranspose(&extTran, kal->extractionMatrix); MatrixMult(&S, S, extTran); MatrixAdd(&S, S, kal->covarianceMatrixZ); // Kalman Gain MatrixInv(&Sinv, S); MatrixMult(&K, kal->covarianceMatrixX, extTran); MatrixMult(&K, K, Sinv); // Figure out mean and covariance results MatrixMult(&x_next, K, y); MatrixAdd(&x_next, kal->meanVector, x_next); MatrixMult(&P_next, kal->covarianceMatrixX, extTran); MatrixMult(&P_next, P_next, Sinv); MatrixMult(&P_next, P_next, kal->extractionMatrix); MatrixMult(&P_next, P_next, kal->covarianceMatrixX); MatrixSub(&P_next, kal->covarianceMatrixX, P_next); // Copy results to the kalmanfilter class CopyMatrixByValue(&kal->meanVector, x_next); CopyMatrixByValue(&kal->covarianceMatrixX, P_next); // Delete matricies so we don't have memory leaks.. DeleteMatrix(&y); DeleteMatrix(&S); DeleteMatrix(&extTran); DeleteMatrix(&K); DeleteMatrix(&Sinv); DeleteMatrix(&x_next); DeleteMatrix(&P_next); }
/*! \fn void MultiNSH(int n, Real *tstop, Real *mratio, Real etavk, * Real *uxNSH, Real *uyNSH, Real *wxNSH, Real *wyNSH) * \brief Multi-species NSH equilibrium * * Input: # of particle types (n), dust stopping time and mass ratio array, and * drift speed etavk. * Output: gas NSH equlibrium velocity (u), and dust NSH equilibrium velocity * array (w). */ void MultiNSH(int n, Real *tstop, Real *mratio, Real etavk, Real *uxNSH, Real *uyNSH, Real *wxNSH, Real *wyNSH) { int i,j; Real *Lambda1,**Lam1GamP1, **A, **B, **Tmp; Lambda1 = (Real*)calloc_1d_array(n, sizeof(Real)); /* Lambda^{-1} */ Lam1GamP1=(Real**)calloc_2d_array(n, n, sizeof(Real)); /* Lambda1*(1+Gamma) */ A = (Real**)calloc_2d_array(n, n, sizeof(Real)); B = (Real**)calloc_2d_array(n, n, sizeof(Real)); Tmp = (Real**)calloc_2d_array(n, n, sizeof(Real)); /* definitions */ for (i=0; i<n; i++){ for (j=0; j<n; j++) Lam1GamP1[i][j] = mratio[j]; Lam1GamP1[i][i] += 1.0; Lambda1[i] = 1.0/(tstop[i]+1.0e-16); for (j=0; j<n; j++) Lam1GamP1[i][j] *= Lambda1[i]; } /* Calculate A and B */ MatrixMult(Lam1GamP1, Lam1GamP1, n,n,n, Tmp); for (i=0; i<n; i++) Tmp[i][i] += 1.0; InverseMatrix(Tmp, n, B); for (i=0; i<n; i++) for (j=0; j<n; j++) B[i][j] *= Lambda1[j]; MatrixMult(Lam1GamP1, B, n,n,n, A); /* Obtain NSH velocities */ *uxNSH = 0.0; *uyNSH = 0.0; for (i=0; i<n; i++){ wxNSH[i] = 0.0; wyNSH[i] = 0.0; for (j=0; j<n; j++){ wxNSH[i] -= B[i][j]; wyNSH[i] -= A[i][j]; } wxNSH[i] *= 2.0*etavk; wyNSH[i] *= etavk; *uxNSH -= mratio[i]*wxNSH[i]; *uyNSH -= mratio[i]*wyNSH[i]; wyNSH[i] += etavk; } free(Lambda1); free_2d_array(A); free_2d_array(B); free_2d_array(Lam1GamP1); free_2d_array(Tmp); return; }
VOID CreateViewMatrix() { MATRIX T, R; /* Temporary matrices. */ /* Put eye at origin. */ Translate(T, -View.eye[0], -View.eye[1], -View.eye[2]); MatrixMult(View.vtrans, View.vtrans, T); /* Align view direction with Z axis. */ ViewRotate(R, View.coi[0] - View.eye[0], View.coi[1] - View.eye[1], View.coi[2] - View.eye[2]); MatrixMult(View.vtrans, View.vtrans, R); }
void EjectedPilotClass::CalculateEjectionVector(EP_VECTOR &result) const { Trotation rot; Tpoint modelEject, worldEject; EP_VECTOR dir; float fudge = 30 * PRANDFloat() * DTR; dir = EP_VECTOR ( (float)cos(EjectAngle()+fudge), 0, -(float)sin(EjectAngle()+fudge) ); _rot.GetTrotation(rot); dir.GetTpoint(modelEject); MatrixMult(&rot, &modelEject, &worldEject); result = EP_VECTOR(worldEject); }
int main( int argc, char *argv[] ) { int n , *mat[ 2 ], i, code, *ans ; FILE *fin ; fin = fopen( argv[ 1 ] , "r" ) ; fscanf( fin, "%d", &n ) ; mat[ 0 ] = malloc( sizeof( int ) * n * n ) ; mat[ 1 ] = malloc( sizeof( int ) * n * n ) ; for( i = 0 ; i < ( n * n ) ; i++ ) { fscanf( fin, "%d", &mat[ 0 ][ i ] ) ; /*printf( "%d", mat[0][i]) ; */ } for( i = 0 ; i < ( n * n ) ; i++ ) { fscanf( fin, "%d", &mat[ 1 ][ i ] ) ; } MatrixMult( mat, n ) ; /* printf( "The answer is." ) ; for( i = 0 ; i < ( n * n ) ; i++ ) { printf( " %d", ans[ i ] ) ; code++ ; if( code == 3 ) printf( "\n" ) ; } */ return 0 ; }
void Perspective(float fovy, float aspect, float zNear, float zFar, float *out) { float y=tanf((fovy/2.0f)*3.14159f/180.0f)*zNear, x=aspect*y; float m[16]; if(!out) return; m[0]=zNear/x; m[1]=0.0f; m[2]=0.0f; m[3]=0.0f; m[4]=0.0f; m[5]=zNear/y; m[6]=0.0f; m[7]=0.0f; m[8]=0.0f; m[9]=0.0f; m[10]=-(zFar+zNear)/(zFar-zNear); m[11]=-1.0f; m[12]=0.0f; m[13]=0.0f; m[14]=-(2.0f*zNear*zFar)/(zFar-zNear); m[15]=0.0f; MatrixMult(m, out, out); }
// Projection matrix functions void InfPerspective(float fovy, float aspect, float zNear, float *out) { float y=tanf((fovy/2.0f)*3.14159f/180.0f)*zNear, x=aspect*y; float nudge=1.0f-(1.0f/(1<<16)); float m[16]; if(!out) return; m[0]=zNear/x; m[1]=0.0f; m[2]=0.0f; m[3]=0.0f; m[4]=0.0f; m[5]=zNear/y; m[6]=0.0f; m[7]=0.0f; m[8]=0.0f; m[9]=0.0f; m[10]=-1.0f*nudge; m[11]=-1.0f; m[12]=0.0f; m[13]=0.0f; m[14]=-2.0f*zNear*nudge; m[15]=0.0f; MatrixMult(m, out, out); }
void test_runtime_api() { try { cuda::Device device(0); for(unsigned int size = 1; size <= 64; size*=2) { const int WIDTH = size*BLOCK_SIZE; const int HEIGHT = size*BLOCK_SIZE; HostMatrix<float> M(WIDTH,HEIGHT); M.fillWithRandomData(); //M.print(std::cout); HostMatrix<float> N(WIDTH,HEIGHT); N.fill_diagonal(2); //N.print(std::cout); HostMatrix<float> P_simple(WIDTH,HEIGHT); { std::stringstream text; text << "CUDA Matrix Multiplication (" << WIDTH << "x" << WIDTH << ") Simple method Multiplication time"; ScopedTimer t(text.str()); MatrixMult(M,N,P_simple, false); } HostMatrix<float> P_complex(WIDTH,HEIGHT); { std::stringstream text; text << "CUDA Matrix Multiplication (" << WIDTH << "x" << WIDTH << ") Complex method Multiplication time"; ScopedTimer t(text.str()); MatrixMult(M,N,P_complex, true); } HostMatrix<float> P_cpu(WIDTH,HEIGHT); { std::stringstream text; text << "CPU Matrix Multiplication (" << WIDTH << "x" << WIDTH << ")Multiplication time"; ScopedTimer t(text.str()); P_cpu = M*N; } std::cout << "------------------------------------------------------" << std::endl; //std::cout << "------------------------CUDA SIMPLE -------------------------" << std::endl; //P_simple.print(std::cout); //std::cout << "----------------------CUDA COMPLEX ---------------------------" << std::endl; //P_complex.print(std::cout); //std::cout << "----------------------CPU ---------------------------" << std::endl; //P_cpu.print(std::cout); } } catch(cuda::cuda_exception& e) { std::cerr << e.what() << std::endl; } }
void OTWDriverClass::CalculateHeadRoll(float headRoll, Tpoint* p_at, Tpoint* p_up, Tpoint* p_rt) { // Gillman was medicated when he requested this head roll... // Oh yeah simple, no problem. Well just do a rotation in 3 space about // arbitrary line, piece of cake, not! // See the section entitled "Rotation Tools" in Graphics Gems, edited by Andrew S. Glassner // for the solution served on a silver platter. Or see Mathematical Elements for // Computer Graphics, 2nd Ed. by Rodgers and Adams. Section 3-9 has a very nice explaination // to this very nontrivial problem. if(headRoll != 0.0F) { mlTrig trigRoll; Trotation R; Tpoint p; float c, s, t; float x, y, z; float scale; scale = 1.0f / (float)sqrt(p_at->x * p_at->x + p_at->y * p_at->y + p_at->z * p_at->z); x = p_at->x * scale; y = p_at->y * scale; z = p_at->z * scale; mlSinCos(&trigRoll, headRoll); c = trigRoll.cos; s = trigRoll.sin; t = 1 - c; R.M11 = t * x * x + c; R.M12 = t * x * y + s * z; R.M13 = t * x * z - s * y; R.M21 = t * x * y - s * z; R.M22 = t * y * y + c; R.M23 = t * y * z + s * x; R.M31 = t * x * z + s * y; R.M32 = t * y * z - s * x; R.M33 = t * z * z + c; MatrixMult (&R, p_rt, &p); p_rt->x = p.x; p_rt->y = p.y; p_rt->z = p.z; MatrixMult (&R, p_up, &p); p_up->x = p.x; p_up->y = p.y; p_up->z = p.z; } }
void OTWDriverClass::PadlockF3_CalcCamera(float dT) { mlTrig tiltTrig; mlTrig panTrig; float tiltSin; float tiltLimit; float term; // if(snapStatus == POSTSNAP && mPadlockTimeout > 0.0F) { // return; // } // 2000-11-13 MODIFIED BY S.G. NEED TO CHECK THE RETURN VALUE. IF TRUE, CALC NEW HEAD POSITION. IF FALSE, NOTHING CHANGED SO DON'T MESS AROUND WITH MY HEAD :-) REMAINING OF FUNCTION INCLUDED IN IF BODY. // PadlockF3_SetCamera(dT); if (PadlockF3_SetCamera(dT)) { if(eyePan < mMinPadPan || eyePan > mMaxPadPan) { term = -(float)sin(180.0F * DTR - fabs(eyePan)); tiltLimit = -(float)asin(sqrt(blindB * blindB - (term * term * blindB * blindB) / (blindA * blindA))); if(eyeTilt > tiltLimit) { eyeTilt = tiltLimit; } } eyeTilt = min(max(eyeTilt, -90.0F * DTR), 25.0F * DTR); eyeHeadRoll = 0.0F; if(eyePan > 90.0F * DTR || eyePan < -90.0F * DTR) { mlSinCos (&panTrig, eyePan); if(eyeTilt > 0.0F) { tiltSin = 0.0F; } else { mlSinCos (&tiltTrig, -eyeTilt); tiltSin = tiltTrig.sin; } eyeHeadRoll = eyePan * panTrig.cos - (eyePan + eyePan * panTrig.cos) * tiltSin; } else if (eyeTilt < 0.0F) { eyeHeadRoll = -eyePan * (float)sin(-eyeTilt); } if (g_bNewPadlock) BuildHeadMatrix(FALSE, YAW_PITCH, eyePan, eyeTilt, 0.0F/*eyeHeadRoll*/); else BuildHeadMatrix(FALSE, YAW_PITCH, eyePan, eyeTilt, eyeHeadRoll); // Combine the head and airplane matrices MatrixMult (&ownshipRot, &headMatrix, &cameraRot); } }
void GetRotationMatrix(M4* result, V3 r) { M4 x; M4 y; M4 zy; M4 z; GetIdentityMatrix(result); GetIdentityMatrix(&x); GetIdentityMatrix(&y); GetIdentityMatrix(&z); RotateX(&x, r.x); RotateY(&y, r.y); MatrixMult(&zy, z, y); MatrixMult(result, x, zy); }
void RSBase3D::UpdateMatrix() { static rvector dir,up,right; dir=-Normalize(m_dir); right=Normalize(CrossProduct(dir,m_up)); up=Normalize(CrossProduct(right,dir)); mat._11=right.x;mat._12=right.y;mat._13=right.z; mat._21=dir.x;mat._22=dir.y;mat._23=dir.z; mat._31=up.x;mat._32=up.y;mat._33=up.z; mat._41=position.x;mat._42=position.y;mat._43=position.z; matm=MatrixMult(MatrixMult(RotateYMatrix(RoleFactor), ScaleMatrixXYZ(ScaleFactorX,ScaleFactorY,ScaleFactorZ)),mat); if(parent)matm=MatrixMult(matm,parent->GetMatrix()); LastFrame=RSFrameCount; }
/********************************************************************** * Function_Name: Cehck Rotation matrix * Return : * Comments : Rw * RwT = I **********************************************************************/ int Transformation::CheckRotationMat( double (&R)[4][4] ) { double RT[4][4] = {{0}}; double R_RT[4][4] = {{0}}; Transpose(RT, R ); MatrixMult(R_RT, RT, R ); try { for(int i = 0; i < 4; i++ ) { for( int j = 0; j < 4; j++ ) { if(i == j ) { if( ( R_RT[i][j] - 1 ) > SOME_SMALL_VALUE ) { cerr << "Given Matrix is not Rotation" << endl; cerr << "R * RT != I " << endl; throw ERR_MAT_NOT_ROTATION; break; } } else { if( R_RT[i][j] > SOME_SMALL_VALUE ) { cerr << "Given Matrix is not Rotation" << endl; cerr << "R * RT != I " << endl; throw ERR_MAT_NOT_ROTATION; break; } } } } } catch(int n) { if( n == ERR_MAT_NOT_ROTATION ) { cout << "Rotation ERROR " << endl; throw ERR_MAT_NOT_ROTATION; } } return EXIT_SUCCESS; }
void GetMM(M4* result, V3 pos, V3 rot) { M4 rotation; M4 translation; GetRotationMatrix(&rotation, rot); GetTranslationMatrix(&translation, pos); MatrixMult(result, translation, rotation); }
task main() { // Initiate BNS Library BNS(); // Create a 3x3 matrix of zeros Matrix mat1; CreateZerosMatrix(&mat1, 3, 3); // Create a 3x3 matrix with some data in it // Set location 1 down, 0 across, to be 16 Matrix mat2; CreateMatrix(&mat2, "1.10 3.40 0; 5 3 2; 0 1 1.234"); SetMatrixAt(&mat2, 1, 0, 16); // Creates a 3x3 identity matrix, then multiply it by 11 Matrix mat3; CreateIdentityMatrix(&mat3, 3); MatrixMultiplyScalar(&mat3, 11); // Print matricies to the debugger console PrintMatrix(&mat1); PrintMatrix(&mat2); PrintMatrix(&mat3); // Matrix Examples: // Matrix determinant float det = MatrixDeterminant(&mat2); writeDebugStreamLine("Matrix Det = %f", det); // Matrix Inverse Matrix inv; MatrixInv(&inv, mat2); writeDebugStream("Inverse "); PrintMatrix(&inv); // Matrix Multiplication Matrix mult; MatrixMult(&mult, mat2, mat3); writeDebugStream("Multiply "); PrintMatrix(mult); // Matrix Addition Matrix add; MatrixAdd(&add, mat2, mat3); writeDebugStream("Add "); PrintMatrix(&add); // Matrix Subtraction Matrix sub; MatrixSub(&sub, mat3, mat2); writeDebugStream("Subtract "); PrintMatrix(&sub); }
void MatrixScale(float x, float y, float z, float *out) { float m[16]; if(!out) return; m[ 0]=x; m[ 1]=0.0f; m[ 2]=0.0f; m[ 3]=0.0f; m[ 4]=0.0f; m[ 5]=y; m[ 6]=0.0f; m[ 7]=0.0f; m[ 8]=0.0f; m[ 9]=0.0f; m[10]=z; m[11]=0.0f; m[12]=0.0f; m[13]=0.0f; m[14]=0.0f; m[15]=1.0f; MatrixMult(m, out, out); }
void Ortho(float left, float right, float bottom, float top, float zNear, float zFar, float *out) { float m[16]; MatrixIdentity(m); m[0*4+0]=2/(right-left); m[1*4+1]=2/(top-bottom); m[2*4+2]=-2/(zFar-zNear); m[3*4+0]=-(right+left)/(right-left); m[3*4+1]=-(top+bottom)/(top-bottom); m[3*4+2]=-(zFar+zNear)/(zFar-zNear); MatrixMult(m, out, out); }
void EjectedPilotClass::CalculateAndSetPositionAndOrientationInCockpit() { Trotation rot; Tpoint modelOffset, worldOffset; AircraftClass *aircraft = (AircraftClass*) vuDatabase->Find(_aircraftId); if (!aircraft) return; // Orient the seat the same way as the plane. _rot[I_ROLL] = aircraft->Roll(); _rot[I_PITCH] = aircraft->Pitch(); _rot[I_YAW] = aircraft->Yaw(); // Get the seat offset in model space. SeatOffset().GetTpoint(modelOffset); // Transform to world space. _rot.GetTrotation(rot); MatrixMult(&rot, &modelOffset, &worldOffset); // Get position of aircraft + model space offset. _pos = EP_VECTOR ( aircraft->XPos(), aircraft->YPos(), aircraft->ZPos() ); _pos += worldOffset; // Velocity is the velocity of the plane. _vel = EP_VECTOR ( aircraft->XDelta(), aircraft->YDelta(), aircraft->ZDelta() ); // Angular velocity is the same as the plane. _aVel[I_ROLL] = aircraft->RollDelta(); _aVel[I_PITCH] = aircraft->PitchDelta(); _aVel[I_YAW] = aircraft->YawDelta(); }
void EjectedPilotClass::CalculateThrustVector(EP_VECTOR &result) const { Trotation rot; Tpoint modelThrust, worldThrust; // Thrust is up. modelThrust.x = 0.0; modelThrust.y = 0.0; modelThrust.z = -1.0; // Transform to world space. _rot.GetTrotation(rot); MatrixMult(&rot, &modelThrust, &worldThrust); // Multiply it by the magnitude result = EP_VECTOR(worldThrust); result *= Mass() * SeatThrust(); }
void BDofNode::Draw(void) { Pmatrix dofRot; Pmatrix R; Ppoint T; mlTrig trig; ShiAssert( dofNumber < TheStateStack.CurrentInstance->ParentObject->nDOFs ); if (dofNumber >= TheStateStack.CurrentInstance->ParentObject->nDOFs ) return; // Set up our free rotation mlSinCos (&trig, TheStateStack.CurrentInstance->DOFValues[dofNumber].rotation); dofRot.M11 = 1.0f; dofRot.M12 = 0.0f; dofRot.M13 = 0.0f; dofRot.M21 = 0.0f; dofRot.M22 = trig.cos; dofRot.M23 = -trig.sin; dofRot.M31 = 0.0f; dofRot.M32 = trig.sin; dofRot.M33 = trig.cos; // Now compose this with the rotation into our parents coordinate system MatrixMult( &rotation, &dofRot, &R ); // Now do our free translation // SCR 10/28/98: THIS IS WRONG FOR TRANSLATION DOFs. "DOFValues" is supposed to // translate along the local x axis, but this will translate along the parent's x axis. // To fix this would require a bit more math (and/or thought). Since it // only happens once in Falcon, I'll leave it broken and put a workaround into // the KC10 object so that the parent's and child's x axis are forced into alignment // by inserting an extra dummy DOF bead. T.x = translation.x + TheStateStack.CurrentInstance->DOFValues[dofNumber].translation; T.y = translation.y; T.z = translation.z; // Draw our subtree TheStateStack.PushAll(); TheStateStack.CompoundTransform( &R, &T ); BSubTree::Draw(); TheStateStack.PopAll(); }
static inline void CorrectHKLsLatC(double LatC[6], double **hklsIn,int nhkls,double **hkls) { double a=LatC[0],b=LatC[1],c=LatC[2],alpha=LatC[3],beta=LatC[4],gamma=LatC[5]; int hklnr; for (hklnr=0;hklnr<nhkls;hklnr++){ double ginit[3]; ginit[0] = hklsIn[hklnr][0]; ginit[1] = hklsIn[hklnr][1]; ginit[2] = hklsIn[hklnr][2]; double SinA = sind(alpha), SinB = sind(beta), SinG = sind(gamma), CosA = cosd(alpha), CosB = cosd(beta), CosG = cosd(gamma); double GammaPr = acosd((CosA*CosB - CosG)/(SinA*SinB)), BetaPr = acosd((CosG*CosA - CosB)/(SinG*SinA)), SinBetaPr = sind(BetaPr); double Vol = (a*(b*(c*(SinA*(SinBetaPr*(SinG)))))), APr = b*c*SinA/Vol, BPr = c*a*SinB/Vol, CPr = a*b*SinG/Vol; double B[3][3]; B[0][0] = APr; B[0][1] = (BPr*cosd(GammaPr)), B[0][2] = (CPr*cosd(BetaPr)), B[1][0] = 0, B[1][1] = (BPr*sind(GammaPr)), B[1][2] = (-CPr*SinBetaPr*CosA), B[2][0] = 0, B[2][1] = 0, B[2][2] = (CPr*SinBetaPr*SinA); double GCart[3]; MatrixMult(B,ginit,GCart); double Ds = 1/(sqrt((GCart[0]*GCart[0])+(GCart[1]*GCart[1])+(GCart[2]*GCart[2]))); hkls[hklnr][0] = ginit[0];hkls[hklnr][1] = ginit[1];hkls[hklnr][2] = ginit[2]; hkls[hklnr][3] = Ds; hkls[hklnr][4] = 0; hkls[hklnr][5] = GCart[0]; hkls[hklnr][6] = GCart[1]; hkls[hklnr][7] = GCart[2]; } }
void QuatMatrix(float in[4], float *out) { float m[16]; float xx, yy, zz, mag; if(!out) return; mag=1.0f/sqrtf(in[0]*in[0]+in[1]*in[1]+in[2]*in[2]+in[3]*in[3]); in[0]*=mag; in[1]*=mag; in[2]*=mag; in[3]*=mag; xx=in[1]*in[1]; yy=in[2]*in[2]; zz=in[3]*in[3]; m[ 0]=1.0f-2.0f*(yy+zz); m[ 1]=2.0f*(in[1]*in[2]+in[0]*in[3]); m[ 2]=2.0f*(in[1]*in[3]-in[0]*in[2]); m[ 3]=0.0f; m[ 4]=2.0f*(in[1]*in[2]-in[0]*in[3]); m[ 5]=1.0f-2.0f*(xx+zz); m[ 6]=2.0f*(in[2]*in[3]+in[0]*in[1]); m[ 7]=0.0f; m[ 8]=2.0f*(in[1]*in[3]+in[0]*in[2]); m[ 9]=2.0f*(in[2]*in[3]-in[0]*in[1]); m[10]=1.0f-2.0f*(xx+yy); m[11]=0.0f; m[12]=0.0f; m[13]=0.0f; m[14]=0.0f; m[15]=1.0f; MatrixMult(m, out, out); }
void StateStackClass::CompoundTransform(const Pmatrix *rot, const Ppoint *pos) { Ppoint tempP; // Compute the rotated translation vector for this object Xlation.x += pos->x * Rotation.M11 + pos->y * Rotation.M12 + pos->z * Rotation.M13; Xlation.y += pos->x * Rotation.M21 + pos->y * Rotation.M22 + pos->z * Rotation.M23; Xlation.z += pos->x * Rotation.M31 + pos->y * Rotation.M32 + pos->z * Rotation.M33; Pmatrix tempM = Rotation; tempP.x = ObjSpaceEye.x - pos->x; tempP.y = ObjSpaceEye.y - pos->y; tempP.z = ObjSpaceEye.z - pos->z; Ppoint tempP2 = ObjSpaceLight; // Composit the camera matrix with the object rotation MatrixMult(&tempM,rot,&Rotation); // Compute the eye point in object space MatrixMultTranspose(rot,&tempP,&ObjSpaceEye); // Compute the light direction in object space. MatrixMultTranspose(rot,&tempP2,&ObjSpaceLight); }
inline void StateStackClass::pvtDrawObject(UInt32 operation, ObjectInstance *objInst, const Pmatrix *rot, const Ppoint *pos, const float sx, const float sy, const float sz, const float scale) { UInt32 clipFlag; float MaxLODRange; static int in = 0; ShiAssert(objInst); PushAll(); // Set up our transformations CompoundTransform(rot,pos); SetWorld(rot,pos); if(operation & OP_WARP) { Pmatrix tempM; ShiAssert((sx > 0.0f) && (sx <= 1.0f)); ShiAssert((sy > 0.0f) && (sy <= 1.0f)); ShiAssert((sz > 0.0f) && (sz <= 1.0f)); Pmatrix stretchM = { sx, 0.f, 0.f, 0.f, sy, 0.f, 0.f, 0.f, sz }; tempM = Rotation; MatrixMult(&tempM,&stretchM,&Rotation); D3DFrame::Matrix mS,mT; mT = mW; mS.InitIdentity(); mS.m[0][0]=sx; mS.m[1][1]=sy; mS.m[2][2]=sz; mW = mS*mT; } if(scale != 1.f) { Pmatrix tempM; Pmatrix scaleM = { scale, 0.f, 0.f, 0.f, scale, 0.f, 0.f, 0.f, scale }; tempM = Rotation; MatrixMult(&tempM,&scaleM,&Rotation); D3DFrame::Matrix mS,mT; mT = mW; mS.InitIdentity(); mS.m[0][0]=scale; mS.m[1][1]=scale; mS.m[2][2]=scale; mW = mS*mT; } // Store the adjusted range for LOD determinations LODRange = Xlation.x * LODBiasInv; // Choose the appropriate LOD of the object to be drawn CurrentInstance = objInst; if (objInst->ParentObject) { if (g_bSlowButSafe && F4IsBadCodePtr((FARPROC) objInst->ParentObject)) // JB 010220 CTD (too much CPU) CurrentLOD = 0; // JB 010220 CTD else // JB 010220 CTD if (objInst->id < 0 || objInst->id >= TheObjectListLength || objInst->TextureSet < 0) // JB 010705 CTD second try { ShiAssert(FALSE); CurrentLOD = 0; } else CurrentLOD = objInst->ParentObject->ChooseLOD(LODRange,&LODused,&MaxLODRange); if(CurrentLOD) { // Decide if we need clipping, or if the object is totally off screen clipFlag = CheckBoundingSphereClipping(); // Continue only if some part of the bounding volume is on screen if (clipFlag != OFF_SCREEN) { // Set the jump pointers to turn on/off clipping if (clipFlag == ON_SCREEN) { Transform = TransformNoClip; DrawPrimJumpTable = DrawPrimNoClipJumpTable; } else { Transform = TransformWithClip; DrawPrimJumpTable = DrawPrimWithClipJumpTable; } // Choose perspective correction or not // if ((Xlation.x > CurrentInstance->Radius() * PERSP_CORR_RADIUS_MULTIPLIER) && // !(CurrentLOD->flags & ObjectLOD::PERSP_CORR)) // { // RenderStateTable = RenderStateTableNPC; // } // else // { RenderStateTable = RenderStateTablePC; // } in ++; if (in == 1) { verts = 0; } // Draw the object CurrentLOD->Draw(); // if (in == 1) // { // if (verts) // { // MonoPrint ("Obj %d:%d %d : %d\n", objInst->id, LODused, (int) MaxLODRange, verts); // } // } in --; } } } PopAll(); }
void BXDofNode::Draw(void) { Pmatrix dofRot; Pmatrix R; Ppoint T; mlTrig trig; ShiAssert( dofNumber < TheStateStack.CurrentInstance->ParentObject->nDOFs ); if (dofNumber >= TheStateStack.CurrentInstance->ParentObject->nDOFs ) return; // Set up our free rotation float dofrot=Process_DOFRot(dofNumber,flags,min,max,multiplier,future); /* float dofrot=TheStateStack.CurrentInstance->DOFValues[dofNumber].rotation; if(flags & XDOF_NEGATE) { dofrot=-dofrot; } if(flags & XDOF_MINMAX) { if(dofrot<min) dofrot=min; if(dofrot>max) dofrot=max; } if(flags & XDOF_SUBRANGE && min!=max) { // rescales dofrot so it is 0.0 at Min and 1.0 at Max // it could exceed those bounds, unless MINMAX is set. dofrot-=min; dofrot/=max-min; // then it get's rescaled below. } dofrot*=multiplier; */ mlSinCos (&trig, dofrot); dofRot.M11 = 1.0f; dofRot.M12 = 0.0f; dofRot.M13 = 0.0f; dofRot.M21 = 0.0f; dofRot.M22 = trig.cos; dofRot.M23 = -trig.sin; dofRot.M31 = 0.0f; dofRot.M32 = trig.sin; dofRot.M33 = trig.cos; // Now compose this with the rotation into our parents coordinate system MatrixMult( &rotation, &dofRot, &R ); // Now do our free translation // SCR 10/28/98: THIS IS WRONG FOR TRANSLATION DOFs. "DOFValues" is supposed to // translate along the local x axis, but this will translate along the parent's x axis. // To fix this would require a bit more math (and/or thought). Since it // only happens once in Falcon, I'll leave it broken and put a workaround into // the KC10 object so that the parent's and child's x axis are forced into alignment // by inserting an extra dummy DOF bead. T.x = translation.x + TheStateStack.CurrentInstance->DOFValues[dofNumber].translation; T.y = translation.y; T.z = translation.z; // Draw our subtree TheStateStack.PushAll(); TheStateStack.CompoundTransform( &R, &T ); BSubTree::Draw(); TheStateStack.PopAll(); }
void AircraftClass::DropFlare (void) { vector pos, posDelta; int type; BombClass *weapon; if (counterMeasureStation[FLARE_STATION].weaponCount > 0) { if (this == FalconLocalSession->GetPlayerEntity()) g_intellivibeData.FlareDropped++; { static int chaffsid=0; // just need a fake id so multiple chaffs can play at once. chaffsid = (chaffsid + 1) & 0xf; SoundPos.Sfx( af->auxaeroData->sndBBFlare, chaffsid); } /* pos.x = XPos(); pos.y = YPos(); pos.z = ZPos(); posDelta.x = XDelta() * 0.75F; posDelta.y = YDelta() * 0.75F; posDelta.z = ZDelta() * 0.75F; */ // MLR 2003-11-16 New positional dispensers int NumToLaunch = 1; if(af->auxaeroData->Flare.Sequence==2) { NumToLaunch=af->auxaeroData->Flare.Count; } int i; for(i=0;i<NumToLaunch && counterMeasureStation[FLARE_STATION].weaponCount > 0;i++) { counterMeasureStation[FLARE_STATION].weaponCount--; Tpoint work; MatrixMult( &((DrawableBSP*)af->platform->drawPointer)->orientation, &af->auxaeroData->Flare.Pos[flareDispenser], &work ); pos.x=work.x + XPos(); pos.y=work.y + YPos(); pos.z=work.z + ZPos(); MatrixMult( &((DrawableBSP*)af->platform->drawPointer)->orientation, &af->auxaeroData->Flare.Vec[flareDispenser], &work ); posDelta.x=work.x + XDelta(); posDelta.y=work.y + YDelta(); posDelta.z=work.z + ZDelta(); switch(af->auxaeroData->Flare.Sequence) { case 0: // alternate dispensers; case 2: flareDispenser++; if(flareDispenser>=af->auxaeroData->Flare.Count) flareDispenser=0; break; case 1: // use 1 dispenser, then move to the next default: flareUsed++; if(flareUsed>=af->auxaeroData->Flare.Decoys[flareDispenser]) { flareUsed=0; flareDispenser++; if(flareDispenser>=af->auxaeroData->Flare.Count) flareDispenser=0; } break; } //type = GetClassID (DOMAIN_AIR, CLASS_VEHICLE, TYPE_BOMB, STYPE_BOMB_IRON, SPTYPE_MK82, VU_ANY, VU_ANY, VU_ANY) + VU_LAST_ENTITY_TYPE; // JB 010220 type = GetClassID (DOMAIN_AIR, CLASS_VEHICLE, TYPE_BOMB, STYPE_FLARE1, SPTYPE_CHAFF1 + 1, VU_ANY, VU_ANY, VU_ANY) + VU_LAST_ENTITY_TYPE; // JB 010220 weapon = new FlareClass (type); weapon->Init(); weapon->SetParent(this); weapon->Start(&pos, &posDelta, 0.2f); vuDatabase->/*Quick*/Insert(weapon); weapon->Wake(); } SetFlareExpireTime( SimLibElapsedTime + FlareTime ); SetNewestFlareID( weapon->Id() ); } //MI for EWS stuff if(g_bRealisticAvionics && this == FalconLocalSession->GetPlayerEntity()) { if(counterMeasureStation[FLARE_STATION].weaponCount == 0) { //F4SoundFXSetDist(af->auxaeroData->sndBBChaffFlareOut, TRUE, 0.0f, 1.0f); SoundPos.Sfx(af->auxaeroData->sndBBChaffFlareOut); //make sure we don't get here again, no sounds from now on counterMeasureStation[FLARE_STATION].weaponCount--; } else if(OTWDriver.pCockpitManager->mpIcp->FlareBingo == counterMeasureStation[FLARE_STATION].weaponCount) { if(OTWDriver.pCockpitManager->mpIcp->EWS_BINGO_ON) SoundPos.Sfx(af->auxaeroData->sndBBChaffFlareLow); //F4SoundFXSetDist( af->auxaeroData->sndBBChaffFlareLow, TRUE, 0.0f, 1.0f ); } //MI moved further down /*else if(counterMeasureStation[FLARE_STATION].weaponCount > 0) F4SoundFXSetDist(SFX_BB_CHAFLARE, FALSE, 0.0f, 1.0f);*/ } // If this is the player and they want unlimited chaff, let 'em have it if (IsSetFlag(MOTION_OWNSHIP) && PlayerOptions.UnlimitedChaff()) counterMeasureStation[FLARE_STATION].weaponCount++; }
void AircraftClass::DropChaff (void) { vector pos, posDelta; int type; BombClass *weapon; if (counterMeasureStation[CHAFF_STATION].weaponCount > 0) { if (this == FalconLocalSession->GetPlayerEntity()) g_intellivibeData.ChaffDropped++; /* pos.x = XPos(); pos.y = YPos(); pos.z = ZPos(); posDelta.x = XDelta() * 0.75F; posDelta.y = YDelta() * 0.75F; posDelta.z = ZDelta() * 0.75F; */ // new positional Dispensers int NumToLaunch = 1; if(af->auxaeroData->Chaff.Sequence==2) { NumToLaunch=af->auxaeroData->Chaff.Count; } int i; for(i=0;i<NumToLaunch && counterMeasureStation[CHAFF_STATION].weaponCount > 0;i++) { counterMeasureStation[CHAFF_STATION].weaponCount--; Tpoint work; MatrixMult( &((DrawableBSP*)af->platform->drawPointer)->orientation, &af->auxaeroData->Chaff.Pos[chaffDispenser], &work ); pos.x=work.x + XPos(); pos.y=work.y + YPos(); pos.z=work.z + ZPos(); MatrixMult( &((DrawableBSP*)af->platform->drawPointer)->orientation, &af->auxaeroData->Chaff.Vec[chaffDispenser], &work ); posDelta.x=work.x + XDelta(); posDelta.y=work.y + YDelta(); posDelta.z=work.z + ZDelta(); switch(af->auxaeroData->Chaff.Sequence) { case 0: // alternate dispensers; case 2: chaffDispenser++; if(chaffDispenser>=af->auxaeroData->Chaff.Count) chaffDispenser=0; break; case 1: // use 1 dispenser, then move to the next default: chaffUsed++; if(chaffUsed>=af->auxaeroData->Chaff.Decoys[chaffDispenser]) { chaffUsed=0; chaffDispenser++; if(chaffDispenser>=af->auxaeroData->Chaff.Count) chaffDispenser=0; } break; } // TODO: Use a different (much higher drag) type for the chaff //type = GetClassID (DOMAIN_AIR, CLASS_SFX, TYPE_CHAFF, STYPE_CHAFF, SPTYPE_CHAFF1, VU_ANY, VU_ANY, VU_ANY) + VU_LAST_ENTITY_TYPE; // JB 010220 type = GetClassID (DOMAIN_AIR, CLASS_VEHICLE, TYPE_BOMB, STYPE_CHAFF, SPTYPE_CHAFF1, VU_ANY, VU_ANY, VU_ANY) + VU_LAST_ENTITY_TYPE; // JB 010220 weapon = new ChaffClass(type); weapon->Init(); weapon->SetParent(this); weapon->Start(&pos, &posDelta, 0.2f); vuDatabase->/*Quick*/Insert(weapon); weapon->Wake(); } SetChaffExpireTime( SimLibElapsedTime + ChaffTime ); SetNewestChaffID( weapon->Id() ); } //MI for EWS stuff if(g_bRealisticAvionics && this == FalconLocalSession->GetPlayerEntity()) { if(counterMeasureStation[CHAFF_STATION].weaponCount == 0) { SoundPos.Sfx(af->auxaeroData->sndBBChaffFlareOut); //make sure we don't get here again, no sounds from now on counterMeasureStation[CHAFF_STATION].weaponCount--; } else if(OTWDriver.pCockpitManager->mpIcp->ChaffBingo == counterMeasureStation[CHAFF_STATION].weaponCount) { if(OTWDriver.pCockpitManager->mpIcp->EWS_BINGO_ON) SoundPos.Sfx(af->auxaeroData->sndBBChaffFlareLow); } //MI Moved further down /*if(counterMeasureStation[CHAFF_STATION].weaponCount > 0) F4SoundFXSetDist(SFX_BB_CHAFLARE, FALSE, 0.0f, 1.0f);*/ } // If this is the player and they want unlimited chaff, let 'em have it if (IsSetFlag(MOTION_OWNSHIP) && PlayerOptions.UnlimitedChaff()) counterMeasureStation[CHAFF_STATION].weaponCount++; }