//----------------------------------------------------------------------------- // Builds a rotation matrix that rotates one direction vector into another //----------------------------------------------------------------------------- void MatrixBuildTranslation( VMatrix& dst, float x, float y, float z ) { MatrixSetIdentity( dst ); dst[0][3] = x; dst[1][3] = y; dst[2][3] = z; }
void MatrixBuildTranslation( VMatrix& dst, const Vector &translation ) { MatrixSetIdentity( dst ); dst[0][3] = translation[0]; dst[1][3] = translation[1]; dst[2][3] = translation[2]; }
void CBaseVSShader::SetVertexShaderMatrix4x4( int vertexReg, int matrixVar ) { IMaterialVar* pTranslationVar = s_ppParams[matrixVar]; if (pTranslationVar) { s_pShaderAPI->SetVertexShaderConstant( vertexReg, &pTranslationVar->GetMatrixValue( )[0][0], 4 ); } else { VMatrix matrix; MatrixSetIdentity( matrix ); s_pShaderAPI->SetVertexShaderConstant( vertexReg, &matrix[0][0], 4 ); } }
//----------------------------------------------------------------------------- // Builds a rotation matrix that rotates one direction vector into another //----------------------------------------------------------------------------- void MatrixBuildRotation( VMatrix &dst, const Vector& initialDirection, const Vector& finalDirection ) { float angle = DotProduct( initialDirection, finalDirection ); assert( IsFinite(angle) ); Vector axis; // No rotation required if (angle - 1.0 > -1e-3) { // parallel case MatrixSetIdentity(dst); return; } else if (angle + 1.0 < 1e-3) { // antiparallel case, pick any axis in the plane // perpendicular to the final direction. Choose the direction (x,y,z) // which has the minimum component of the final direction, use that // as an initial guess, then subtract out the component which is // parallel to the final direction int idx = 0; if (FloatMakePositive(finalDirection[1]) < FloatMakePositive(finalDirection[idx])) idx = 1; if (FloatMakePositive(finalDirection[2]) < FloatMakePositive(finalDirection[idx])) idx = 2; axis.Init( 0, 0, 0 ); axis[idx] = 1.0f; VectorMA( axis, -DotProduct( axis, finalDirection ), finalDirection, axis ); VectorNormalize(axis); angle = 180.0f; } else { CrossProduct( initialDirection, finalDirection, axis ); VectorNormalize( axis ); angle = acos(angle) * 180 / M_PI; } MatrixBuildRotationAboutAxis( dst, axis, angle ); #ifdef _DEBUG Vector test; Vector3DMultiply( dst, initialDirection, test ); test -= finalDirection; assert( test.LengthSqr() < 1e-3 ); #endif }
CLightingManager::CLightingManager() : BaseClass( "LightingManagerSystem" ) { #if DEBUG m_bVolatileLists = false; #endif m_bDrawWorldLights = true; MatrixSetIdentity( m_matScreenToWorld ); m_vecViewOrigin.Init(); m_vecForward.Init(); m_flzNear = 0; m_bDrawVolumetrics = false; #if DEFCFG_USE_SSE m_pSortDataX4 = NULL; m_uiSortDataCount = 0; #endif }
END_SHADER_PARAMS SHADER_INIT_PARAMS() { INIT_FLOAT_PARM( MAXDISTANCE, 100000.0 ); INIT_FLOAT_PARM( FARFADEINTERVAL, 400.0 ); INIT_FLOAT_PARM( MAXSIZE, 20.0 ); INIT_FLOAT_PARM( ENDFADESIZE, 20.0 ); INIT_FLOAT_PARM( STARTFADESIZE, 10.0 ); INIT_FLOAT_PARM( DEPTHBLENDSCALE, 50.0 ); INIT_FLOAT_PARM( OVERBRIGHTFACTOR, 1.0 ); INIT_FLOAT_PARM( ADDBASETEXTURE2, 0.0 ); INIT_FLOAT_PARM( ADDSELF, 0.0 ); INIT_FLOAT_PARM( ZOOMANIMATESEQ2, 0.0 ); INIT_FLOAT_PARM( ALPHATRAILFADE, 1. ); INIT_FLOAT_PARM( RADIUSTRAILFADE, 1. ); INIT_FLOAT_PARM( VERTEXFOGAMOUNT, 0.0f ); INIT_FLOAT_PARM( OUTLINEALPHA, 1.0 ); if ( !params[ORIENTATIONMATRIX]->IsDefined() ) { VMatrix mat; MatrixSetIdentity( mat ); params[ORIENTATIONMATRIX]->SetMatrixValue( mat ); } if ( !params[CROPFACTOR]->IsDefined() ) { params[CROPFACTOR]->SetVecValue( 1.0f, 1.0f ); } if ( !params[DEPTHBLEND]->IsDefined() ) { params[ DEPTHBLEND ]->SetIntValue( GetDefaultDepthFeatheringValue() ); } if ( !g_pHardwareConfig->SupportsPixelShaders_2_b() ) { params[ DEPTHBLEND ]->SetIntValue( 0 ); } InitIntParam( DUALSEQUENCE, params, 0 ); InitIntParam( MAXLUMFRAMEBLEND1, params, 0 ); InitIntParam( MAXLUMFRAMEBLEND2, params, 0 ); InitIntParam( EXTRACTGREENALPHA, params, 0 ); InitIntParam( ADDOVERBLEND, params, 0 ); InitIntParam( BLENDFRAMES, params, 1 ); InitIntParam( DISTANCEALPHA, params, 0 ); InitIntParam( OUTLINE, params, 0 ); InitIntParam( SOFTEDGES, params, 0 ); InitIntParam( PERPARTICLEOUTLINE, params, 0 ); if ( !params[USEINSTANCING]->IsDefined() ) { params[ USEINSTANCING ]->SetIntValue( IsX360() ? 1 : 0 ); } // srgb read 360 InitIntParam( SHADERSRGBREAD360, params, 0 ); // default to being translucent since that's what we always were for historical reasons. InitIntParam( OPAQUE, params, 0 ); InitIntParam( VERTEXCOLORLERP, params, 0 ); if ( !params[LERPCOLOR1]->IsDefined() ) { params[LERPCOLOR1]->SetVecValue( 1.0f, 0.0f, 0.0f ); } if ( !params[LERPCOLOR2]->IsDefined() ) { params[LERPCOLOR2]->SetVecValue( 0.0f, 1.0f, 0.0f ); } if ( params[OPAQUE]->GetIntValue() != 0 ) { // none of these make sense if we have $opaque 1: params[ADDBASETEXTURE2]->SetFloatValue( 0.0f ); params[DUALSEQUENCE]->SetIntValue( 0 ); params[SEQUENCE_BLEND_MODE]->SetIntValue( 0 ); params[MAXLUMFRAMEBLEND1]->SetIntValue( 0 ); params[MAXLUMFRAMEBLEND2]->SetIntValue( 0 ); params[EXTRACTGREENALPHA]->SetIntValue( 0 ); params[RAMPTEXTURE]->SetUndefined(); params[ZOOMANIMATESEQ2]->SetIntValue( 0 ); params[ADDOVERBLEND]->SetIntValue( 0 ); params[ADDSELF]->SetIntValue( 0 ); params[BLENDFRAMES]->SetIntValue( 0 ); params[DEPTHBLEND]->SetIntValue( 0 ); params[INVERSEDEPTHBLEND]->SetIntValue( 0 ); } SET_FLAGS2( MATERIAL_VAR2_IS_SPRITECARD ); }
static int vmatrix_MatrixSetIdentity (lua_State *L) { MatrixSetIdentity(luaL_checkvmatrix(L, 1)); return 0; }
int main(void) { // <editor-fold defaultstate="collapsed" desc="Initialization of HW components/modules"> //******************************************************************* // Initialization of HW components/modules //=================================================================== Init(); TMRInit(2); // Initialize Timer interface with Priority=2 BLIInit(); // Initialize Signal interface //=================================================================== BLIAsyncStart(50, 50); // Fast blinking - initialization //=================================================================== MCMInitT(2.5, 2500); // Initialize Motor Control for PPM with setting // Throttle to HIGH for delay interval to let ESC // capture Throttle range //-------------------------- ADCInit(3); // Initialize ADC to control battery //-------------------------- RCInit(4); // Initialize Receiver interface with Priority=4 //-------------------------- I2CInit(5, 1); // Initialize I2C1 module with IPL=5 and Fscl=400 KHz //-------------------------- UARTInitTX(6, 350); // Initialize UART1 for TX on IPL=6 at // BaudRate = 48 => 115,200 bps - ZigBEE //-------------------------------------- // BaudRate = 100 => 250,000 bps // BaudRate = 200 => 500,000 bps // BaudRate = 250 => 625,000 bps // BaudRate = 350 => 833,333 bps - SD Logger, FTDI // BaudRate = 500 => 1,250,000 bps // BaudRate = 1000 => 2,500,000 bps //******************************************************************* // Initialize Magnetometer //-------------------------------------------------------------- if (HMCInit(6, 1, 0)) // Initialize magnetic Sensor // ODR = 6 (max, 75 Hz), // Gain = 1 (1.3 Gs) // DLPF = 0 (no averaging) BLIDeadStop("EM", 2); //================================================================== // Initialize motion sensor - No calibration at this time!!! //------------------------------------------------------------------ if ( MPUInit(0, 3) ) // Initialize motion Sensor // 1 kHz/(0+1) = 1000 Hz (1ms) // DLPF=3 => Bandwidth 44 Hz (delay: 4.9 msec) BLIDeadStop("EA", 2); //------------------------------------------------------------------ //================================================================== AltimeterInit(4); // Initialize Altimeter with IPL=4 (if needed) // NOTE: All sensors areinitialized, so no more synchronous I2C // operations are needed. Thus we may start Altimeter to give // it more time to "warm-up" AltimeterReset(); // NOTE: will be reset again later //================================================================== BLIAsyncStop(); // Initialization complete //=================================================================== // </editor-fold> //******************************************************************* // Quadrocopter control variables //------------------------------------------------------------------- // <editor-fold defaultstate="collapsed" desc="Timing control variables"> ulong StartTS = 0; // Flight start time ulong StepTS; // Control Loop step start time ulong Alarm = 0; // "alarm" variable used to set // the duration of the Control Loop // </editor-fold> // <editor-fold defaultstate="collapsed" desc="Variable required to process RC Feed "> //------------------------------------------------------------------- RCData RCFeed; // Control input from Receiver adjusted // by control rates. It may also be // corrected (if necessary) to account // for "+" or "X" configuration as // specified by MODE_CB_to_MF flag //--------------------------------------- RCData RCSmthd; // Smoothed control input used in all // subsequent control calculations //--------------------------------------- RCData RCFinal; // Smoothed RC input adjusted for the // Yaw angle to provide "Course Lock" (if // required). Final RC input provided to // Quarocopter Control Module //------------------------------------------------------------------- // We need battery Nominal voltage to adjust RC Throttle as battery // charge depletes. float BatNomV = ADCGetBatteryNomVoltage (); //------------------------------------------------------------------- Matrix Mtr_CB_To_MF; // Rotation matrix used to adjust Control // Board orientation to Model front //--------------------------------------- Matrix Mtr_CrsLck; // Rotation matrix used to adjust RC input // to account for current Yaw to implement // "Course Lock" //--------------------------------------- if (MODE_CB_to_MF) // Initialize Rotation Matrix for pre-set angle MatrixYawRotation(__CB_To_MF_Angle__, &Mtr_CB_To_MF); else // Reset Rotation matrix to Identity MatrixSetIdentity(&Mtr_CB_To_MF); //------------------------------------------------------------------- // </editor-fold> DCMData IMU; // Orientation data from DCM MCMData MC; // Motor control Variables //------------------------------------------------------------------- // <editor-fold defaultstate="collapsed" desc="ARMing RC and initializing IMU"> //================================================================== // Wait for the Receiver ARMing: Throttle should go up and then down //------------------------------------------------------------------ BLIAsyncStart(200, 200); // Really slow RCArm(); BLIAsyncStop(); //================================================================== // </editor-fold> Re_Start: //================================================================== // <editor-fold defaultstate="collapsed" desc="Intialize Re-Start"> //------------------------------------------------------------------ StartTS = 0; // Reset timing of control Loop //------------------------------------------------------------------ BLIAsyncMorse( "I", 1); // dit - dit RCReadWhenReady (&RCFeed); // Get reading from receiver //------------------------------------------------------------------ // Safety check: CONTROL should be UP and THROTTLE - LOW //------------------------------------------------------------------ while ( 0 == RCFeed.Control || RCFeed.Throttle > 0.1 ) { RCReadWhenReady (&RCFeed); // Get next reading from receiver } //------------------------------------------------------------------ BLIAsyncStop (); //================================================================== //================================================================== // Normalize last read RC values as they will represent the first // input into the control algorithm, so should be adjusted accordingly //------------------------------------------------------------------ NormalizeRCFeed(&RCFeed); //------------------------------------------------------------------ // Reset Smothed RC data (initialize filter) //------------------------------------------------------------------ RCDataReset(&RCSmthd); //================================================================== // Initialize sensors... Performed after ARMing to ensure that model // is stable //-------------------------------------------------------------- BLIAsyncStart(100, 50); //------------------------------------------------------------------ // Start IMU and wait until orientation estimate stabilizes //------------------------------------------------------------------ IMUReset(); //-------------------------------------------------------------- BLIAsyncStart(50, 100); //------------------------------------------------------------------ // Reset Altimeter //------------------------------------------------------------------ AltimeterReset(); //------------------------------------------------------------------ QCMReset (); // Initialize (reset) QCM variables //------------------------------------------------------------------ // Force update of DCMData as inside the control Loop this // call is non-blocking! //------------------------------------------------------------------ if (IMU_OK != IMUGetUpdateWhenReady (&IMU)) // Failure... BLIDeadStop ("A", 1); //-------------------------------------------------------------- BLIAsyncStop(); //-------------------------------------------------------------- // </editor-fold> //================================================================== //******************************************************************* // Quadrocopter Control Loop //------------------------------------------------------------------- BLISignalON(); while (1) { //============================================================ // Timing of control Loop //============================================================ // <editor-fold defaultstate="collapsed" desc="Control Loop timing management"> // Sets the "frquency" of Control Loop Alarm = TMRSetAlarm (10); // Set Alarm 10 msec in the future //------------------------------------------------------------ StepTS = TMRGetTS (); // Capture TS of this step if ( 0 == StartTS ) // First iteration of Control? StartTS = StepTS; // Yes, capture timestamp // </editor-fold> //============================================================ //============================================================ // Read commands from receiver - non-blocking call! // (we will get out of this call even if connection to the // receiver is lost!) // At the end processed Receiver data is stored in RCSmthd //============================================================ // <editor-fold defaultstate="collapsed" desc="Process Receiver feed"> if ( RCRead(&RCFeed) ) { //------------------------------------------------------------ // Every NEW sample need to be "normalized" //------------------------------------------------------------ NormalizeRCFeed(&RCFeed); //------------------------------------------------------------ // Adjust orientation from "+" to "X" if needed //------------------------------------------------------------ if (MODE_CB_to_MF) { // Control board front does not coincide with the model front Vector RCInput; Vector RCRotated; VectorSet(RCFeed.Roll, RCFeed.Pitch, RCFeed.Yaw, &RCInput); MatrixTimesVector(&Mtr_CB_To_MF, &RCInput, &RCRotated); RCFeed.Roll = RCRotated.X; RCFeed.Pitch = RCRotated.Y; RCFeed.Yaw = RCRotated.Z; // NOTE: Yaw is not affected! } } //============================================================ // Smooth RC data //------------------------------------------------------------ // Roll, Pitch, and Yaw are smoothed with the IIR(4) //------------------------------------------------------------ RCSmthd.Roll = (RCSmthd.Roll * 3.0 + RCFeed.Roll ) * 0.25; // 1/4 = 0.25 RCSmthd.Pitch = (RCSmthd.Pitch* 3.0 + RCFeed.Pitch) * 0.25; if (MODE_Course_Lock) { // In this mode Yaw is integrated over the Control Loop // duration (0.01 sec) and normalized to +/-Pi range RCSmthd.Yaw = QCMRangeToPi(RCSmthd.Yaw + RCFeed.Yaw * 0.01); } else { // Without Course-Lock Yaw is treated as "direct input" and // just smothed similar to other control variables. RCSmthd.Yaw = (RCSmthd.Yaw * 3.0 + RCFeed.Yaw ) * 0.25; } //------------------------------------------------------------ // Throttle is smoothed with the IIR(4) and adjusted to // account for actual battery voltage. This is done to // improve "hovering" when throttle stick is not moving. //------------------------------------------------------------ // Adjust Native (from RC) throttle to a value corresponding float BatV = ADCGetBatteryVoltage(); float BatAdjTh = RCFeed.Throttle; if (BatV > 2.0) // Sanity check :) BatAdjTh = BatAdjTh * BatNomV / BatV; //----------------------------------------- RCSmthd.Throttle = (RCSmthd.Throttle*3.0 + BatAdjTh) * 0.25; // 1/4 = 0.25 //----------------------------------------- RCSmthd.Control = RCFeed.Control; // </editor-fold> //============================================================ //============================================================ // Implement Motor Cut-Off if RC Control is LOW //============================================================ // <editor-fold defaultstate="collapsed" desc="Process RC Control"> if ( 0 == RCSmthd.Control ) { // Yes, Control is reliably low! //-------------------------------------------- // Override motor control //-------------------------------------------- MC.F = MC.B = MC.L = MC.R = 0; MCMSet(&MC); //-------------------------------------------- // Flight terminated, post EOF to Data Logger //-------------------------------------------- TMRDelay(10); // Wait for pipe to clear UARTPostIfReady( NULL, 0); // ... and again - to be sure! TMRDelay(10); // Wait for pipe to clear UARTPostIfReady( NULL, 0); //---------------------------------- goto Re_Start; } // </editor-fold> //============================================================ //============================================================ // Obtain and process battery charge status //============================================================ // <editor-fold defaultstate="collapsed" desc="Process Battery level"> float BatteryCharge = QCMBatteryMgmt(); if (BatteryCharge < 0.35) // BC < 35% { float MaxLevel = 2.0 * BatteryCharge; if (RCSmthd.Throttle > MaxLevel) RCSmthd.Throttle = MaxLevel; } // </editor-fold> //============================================================ //============================================================ // Read current orientation from the IMU with sensors' raw // measurements (non-blocking call) and, if necessary, perform // adjustment to RCSmthd to obtain RCFinal //============================================================ // <editor-fold defaultstate="collapsed" desc="Process IMU data and generate Motor Control"> // First, unconditionally promote RCSmthd to RCFinal RCDataCopy(&RCSmthd, &RCFinal); //------------------------------------------------------------ if (IMU_OK == IMUGetUpdate(&IMU)) { if (MODE_Course_Lock) { // We need to rotate RC input to adjust for current Yaw // to implement "Course Lock" - RC Roll and Pitch commands // are interpreted as being applied to the original (pre- // takeoff) orientation of the model //------------------------------------------------------- Vector RCInput; Vector RCRotated; // Build rotation matrix to adjust for orientation discrepancy MatrixYawRotation(-IMU.Yaw, &Mtr_CrsLck); // Load RC input into RCInput vector VectorSet(RCSmthd.Roll, RCSmthd.Pitch, RCSmthd.Yaw, &RCInput); // Rotate RCInput vector MatrixTimesVector(&Mtr_CrsLck, &RCInput, &RCRotated); // Save rotated values RCFinal.Roll = RCRotated.X; RCFinal.Pitch = RCRotated.Y; RCFinal.Yaw = RCRotated.Z; } // Calculate motor control based // upon IMU data QCMPerformStep(&RCFinal, &IMU, &MC); } else { // IMU Failed to provide update - // set Motor Control to RC throttle MC.F = MC.B = MC.L = MC.R = RCFinal.Throttle; } // </editor-fold> //============================================================ //************************************************************ // Implement Motor Cut-Off to protect props if model is // dangerously tilted (> 60 degrees) while RC Throttle is low //------------------------------------------------------------ if (IMU.Incl <= 0.5 && RCFinal.Throttle <= 0.40) { // Override motor control MC.F = MC.B = MC.L = MC.R = 0.0; } //------------------------------------------------------------ //************************************************************ // Update motor control //************************************************************ MCMSet(&MC); //------------------------------------------------------------ //=========================================================== // Load and post telemetry data (non-blocking call) //----------------------------------------------------------- // <editor-fold defaultstate="collapsed" desc="Populating Telemetry"> //----------------------------------------- TM.TS = StepTS - StartTS; //---------------------- TM.Roll = IMU.Roll; TM.Pitch = IMU.Pitch; TM.Yaw = IMU.Yaw; TM.Inclination = IMU.Incl; TM.Azimuth = IMU.Azimuth; //---------------------- #ifdef _TMReport_GYRO_ #ifdef _TMReport_GYRO_Rotated //------------------------------------------------------ // We rotate Gyro vector using "partial" DCM built using // only Roll and Pitch angles as the actual Yaw does not // affect Angualr Velocity by axis //------------------------------------------------------ Vector Earth; // One of the Earth axis in Body frame Vector Cross; // Temporary vector to hold the cross // product //------------------------------------------------------ // Calculate Roll and Pitch rates //------------------------------------------------------ // Retrieve Earth Z-axis and calculate cross-product of // Z-axis with Gyro vector VectorCrossProduct(DCMEarthZ(&Earth), &IMU.GyroRate, &Cross); TM.Gyro.X = Cross.X; TM.Gyro.Y = Cross.Y; //------------------------------------------------------ // Calculate Yaw rate //------------------------------------------------------ // Retrieve Earth X-axis and calculate cross-product of // X-axis with Gyro vector VectorCrossProduct(DCMEarthX(&Earth), &IMU.GyroRate, &Cross); TM.Gyro.Z = Cross.Z; #else // Just report native Gyro data VectorCopy(&IMU.GyroRate, &TM.Gyro); #endif #endif //---------------------- #ifdef _TMReport_ACC_ // Just report native Gyro data VectorCopy(&IMU.Gravity, &TM.Acc); #endif //---------------------- TM.RollDer = QSD.RollDer; TM.PitchDer = QSD.PitchDer; TM.YawDer = QSD.YawDer; //---------------------- TM.RC_Throttle = RCFinal.Throttle; TM.RC_Roll = RCFinal.Roll; TM.RC_Pitch = RCFinal.Pitch; TM.RC_Yaw = RCFinal.Yaw; //---------------------- #ifdef _TMReport_RCSmthd_ TM.RCS_Throttle = RCSmthd.Throttle; TM.RCS_Roll = RCSmthd.Roll; TM.RCS_Pitch = RCSmthd.Pitch; TM.RCS_Yaw = RCSmthd.Yaw; #endif //---------------------- #ifdef _TMReport_RCFeed_ TM.RCN_Throttle = RCFeed.Throttle; TM.RCN_Roll = RCFeed.Roll; TM.RCN_Pitch = RCFeed.Pitch; TM.RCN_Yaw = RCFeed.Yaw; #endif //---------------------- #ifdef _TMReport_Altimetry_ if ( 0 == AltimeterGetAltData( IMU.TS, &IMU.Gravity, &TM.AltResult) ) goto EndOfCycle; // Skip reporting on this step #endif //---------------------- #ifdef _TMReport_PID_ #ifdef _TMReport_PID_Details TM.DRProp = QSD.DeltaRollProp; TM.DRDiff = QSD.DeltaRollDiff; TM.DRInt = QSD.DeltaRollInt; #endif TM.DRTot = QSD.DeltaRoll; //------------- #ifdef _TMReport_PID_Details TM.DPProp = QSD.DeltaPitchProp; TM.DPDiff = QSD.DeltaPitchDiff; TM.DPInt = QSD.DeltaPitchInt; #endif TM.DPTot = QSD.DeltaPitch; //------------- #ifdef _TMReport_PID_Details TM.DYProp = QSD.DeltaYawProp; TM.DYDiff = QSD.DeltaYawDiff; TM.DYInt = QSD.DeltaYawInt; #endif TM.DYTot = QSD.DeltaYaw; #endif //---------------------- TM.Throttle = QSD.Throttle; // Real Throttle //---------------------- TM.Voltage = ADCGetBatteryVoltage(); // </editor-fold> //=========================================================== UARTPostIfReady( (unsigned char *) &TM, sizeof(TM) ); //=========================================================== // Insert controlled "delay" to slow down the Control Loop // to pre-set frequency EndOfCycle: TMRWaitAlarm(Alarm); //=========================================================== } //******************************************************************* return 1; }