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