bool History::GetSimilar(string &strStr, int LastCmdPartLength, bool bAppend)
{
int Length=(int)strStr.GetLength();
if (LastCmdPartLength!=-1 && LastCmdPartLength<Length)
Length=LastCmdPartLength;
if (LastCmdPartLength==-1)
{
ResetPosition();
}
for (HistoryRecord *HistoryItem=HistoryList.Prev(CurrentItem); HistoryItem != CurrentItem; HistoryItem=HistoryList.Prev(HistoryItem))
{
if (!HistoryItem)
continue;
if (!StrCmpNI(strStr,HistoryItem->strName,Length) && StrCmp(strStr,HistoryItem->strName))
{
if (bAppend)
strStr += &HistoryItem->strName[Length];
else
strStr = HistoryItem->strName;
CurrentItem = HistoryItem;
return true;
}
}
return false;
}
// -----------------------------------------------------------------------------
// CBTGPSFix::ResetFix
// -----------------------------------------------------------------------------
void CBTGPSFix::ResetFix()
{
TRACESTRING("CBTGPSFix::ResetFix start...")
TRealX nan;
nan.SetNaN();
ResetPosition();
iSpeed=nan;
iHeading=nan;
iUsedSatellitesArray.Reset();
iSatelliteTime = TTime(0);
iHorizontalDoP = nan;
iVerticalDoP = nan;
iPositionDoP = nan;
iSatellitesUsed = 0;
//Clear the mask
iReceivedMsgMask = 0;
iGpsMode = EGpsModeSatellite;
TRACESTRING("CBTGPSFix::ResetFix end")
}
void Node::mouseReleaseEvent(QGraphicsSceneMouseEvent *e)
{
if(is_moved==true)
{
emit ResetPosition(e->scenePos());
is_moved=false;
}
}
//---------------------------------------------------//
// [9/16/2009 Albert]
// Description: 设置更新函数
//---------------------------------------------------//
bool CDynamicObject::SetUpdate( _tfunction &Fn, unsigned short repeat, unsigned int timer, unsigned int delay )
{
StopUpdate();
ResetPosition();
// delay 参数为 -1 则表示立即被执行
m_hUpdateTimer = ThisTimer()->insert_event( Fn, repeat, timer, delay );
return m_hUpdateTimer != INVALID_TIMER_HANDLE;
}
//-----------------------------------------------------------------------------
//! 初期化
//-----------------------------------------------------------------------------
bool AllyBase::Initialize()
{
// プレイヤーの初期位置に設定する
_leaderPos = Vector3(-3200.0f, 0.0f, -2600.0f);
// 座標設定
ResetPosition();
// 初期化
Man::Initialize();
return true;
}
Stars::Stars(int num, float spread): vlist(NULL),spread(spread) {
static string starspritetextures = vs_config->getVariable("graphics","near_stars_sprite_texture","");
static float starspritesize = XMLSupport::parse_float(vs_config->getVariable("graphics","near_stars_sprite_size","2"));
if (starspritetextures.length()==0) {
vlist = new PointStarVlist((num/STARnumvlist)+1,spread,"");
} else {
vlist = new SpriteStarVlist((num/STARnumvlist)+1,spread,"",starspritetextures,starspritesize);
}
fade = blend=true;
ResetPosition(QVector(0,0,0));
}
/*
SaveForbid - принудительно запретить запись добавляемой строки.
Используется на панели плагина
*/
void History::AddToHistory(const string& Str, history_record_type Type, const GUID* Guid, const wchar_t *File, const wchar_t *Data, bool SaveForbid)
{
_ASSERTE(this!=NULL);
if (!m_EnableAdd || SaveForbid)
return;
if (Global->CtrlObject->Macro.IsExecuting() && Global->CtrlObject->Macro.IsHistoryDisable((int)m_TypeHistory))
return;
if (m_TypeHistory!=HISTORYTYPE_DIALOG && (m_TypeHistory!=HISTORYTYPE_FOLDER || !Guid || *Guid == FarGuid) && Str.empty())
return;
bool Lock = false;
string strName(Str),strGuid,strFile(NullToEmpty(File)),strData(NullToEmpty(Data));
if(Guid) strGuid=GuidToStr(*Guid);
unsigned __int64 DeleteId = 0;
const bool ignore_data = m_TypeHistory == HISTORYTYPE_CMD;
if (m_RemoveDups) // удалять дубликаты?
{
DWORD index=0;
string strHName,strHGuid,strHFile,strHData;
history_record_type HType;
bool HLock;
unsigned __int64 id;
unsigned __int64 Time;
while (HistoryCfgRef()->Enum(index++,m_TypeHistory,m_HistoryName,&id,strHName,&HType,&HLock,&Time,strHGuid,strHFile,strHData))
{
if (EqualType(Type,HType))
{
typedef int (*CompareFunction)(const string&, const string&);
CompareFunction CaseSensitive = StrCmp, CaseInsensitive = StrCmpI;
CompareFunction CmpFunction = (m_RemoveDups == 2 ? CaseInsensitive : CaseSensitive);
if (!CmpFunction(strName, strHName) &&
!CmpFunction(strGuid, strHGuid) &&
!CmpFunction(strFile, strHFile) &&
(ignore_data || !CmpFunction(strData, strHData)))
{
Lock = Lock || HLock;
DeleteId = id;
break;
}
}
}
}
HistoryCfgRef()->DeleteAndAddAsync(DeleteId, m_TypeHistory, m_HistoryName, strName, Type, Lock, strGuid, strFile, strData); //Async - should never be used in a transaction
ResetPosition();
}
bool History::DeleteIfUnlocked(unsigned __int64 id)
{
bool b = false;
if (id && !HistoryCfgRef()->IsLocked(id))
{
if (HistoryCfgRef()->Delete(id))
{
b = true;
ResetPosition();
}
}
return b;
}
// pre process children
bool opNode::PreProcessChildren()
{
//iterator i = GetBegin();
iterator end = GetEnd();
bool result = true;
ResetPosition();
//while (i != end)
while (GetPosition() != end)
{
//result = i->PreProcess(tracker) ? result : false;
result = GetPosition()->PreProcess() ? result : false;
//++i;
IncrementPosition();
}
ResetPosition();
return result;
}
//************************************************************************
// Called when the configuration has changed
//************************************************************************
void CContactlistScreen::OnConfigChanged()
{
CScreen::OnConfigChanged();
m_ContactList.OnConfigChanged();
m_ContactList.SetDrawTreeLines(CConfig::GetBoolSetting(CLIST_DRAWLINES));
m_ContactList.SetSize(GetWidth()-5, GetHeight()-(CConfig::GetBoolSetting(SHOW_LABELS)?6:0));
m_ContactList.SetFont(CConfig::GetFont(FONT_CLIST));
m_ContactList.SetColumns(CConfig::GetBoolSetting(CLIST_COLUMNS)?2:1);
m_Scrollbar.SetSize(4,GetHeight()-((CConfig::GetBoolSetting(SHOW_LABELS)?5:0)));
ResetPosition();
}
//-----------------------------------------------------------------------------
//! 初期化
//-----------------------------------------------------------------------------
bool EnemyBase::Initialize()
{
if( _keyType != KEY_LEADER) {
_goalPos = Vector3(0.0f);
}else{
_goalPos = IObjDataManager()->_deffencePos[0];
}
// 座標設定
ResetPosition();
// 初期化
Man::Initialize();
return true;
}
SndSysSpeexSoundStream::SndSysSpeexSoundStream (csRef<SndSysSpeexSoundData> pData,
csSndSysSoundFormat *pRenderFormat,
int Mode3D) :
SndSysBasicStream(pRenderFormat, Mode3D), m_pSoundData(pData), header(0)
{
// Allocate an advance buffer
m_pCyclicBuffer = new SoundCyclicBuffer (
(m_RenderFormat.Bits/8 * m_RenderFormat.Channels) *
(m_RenderFormat.Freq * SPEEX_BUFFER_LENGTH_MULTIPLIER /
SPEEX_BUFFER_LENGTH_DIVISOR));
CS_ASSERT(m_pCyclicBuffer!=0);
// Initialize speex stream.
ResetPosition(false);
}
void CUIChildQuickSlot::ResetSavePosition( PIX pixMinI, PIX pixMinJ, PIX pixMaxI, PIX pixMaxJ )
{
g_iXPosQuickSlotEX1 = 0;
g_iYPosQuickSlotEX1 = 0;
g_iXPosQuickSlotEX2 = 0;
g_iYPosQuickSlotEX2 = 0;
g_bQuickSlot1HorOrVer = 1;
g_bQuickSlot1Lock = 0;
g_bQuickSlot2HorOrVer = 1;
g_bQuickSlot2Lock = 0;
RotationRectSet();
ResetPosition( pixMinI, pixMinJ, pixMaxI, pixMaxJ );
SetPos(m_nStartX, m_nStartY);
}
void Stars::UpdatePosition(const QVector & cp) {
if (fabs(pos[0].i-cp.i)>3*spread||fabs(pos[0].j-cp.j)>3*spread||fabs(pos[0].k-cp.k)>3*spread) {
ResetPosition(cp);
return;
}
upd (pos[0].i,pos[1].i,pos[2].i, pos[3].i,pos[4].i,pos[5].i,pos[6].i, pos[7].i,pos[8].i, cp.i, spread);
upd (pos[9].i,pos[10].i,pos[11].i, pos[12].i,pos[13].i,pos[14].i,pos[15].i, pos[16].i,pos[17].i, cp.i, spread);
upd (pos[18].i,pos[19].i,pos[20].i, pos[21].i,pos[22].i,pos[23].i,pos[24].i, pos[25].i,pos[26].i, cp.i, spread);
upd (pos[0].j,pos[1].j,pos[2].j, pos[9].j,pos[10].j,pos[11].j,pos[18].j, pos[19].j,pos[20].j, cp.j, spread);
upd (pos[3].j,pos[4].j,pos[5].j, pos[12].j,pos[13].j,pos[14].j,pos[21].j, pos[22].j,pos[23].j, cp.j, spread);
upd (pos[6].j,pos[7].j,pos[8].j, pos[15].j,pos[16].j,pos[17].j,pos[24].j, pos[25].j,pos[26].j, cp.j, spread);
upd (pos[0].k,pos[3].k,pos[6].k, pos[9].k,pos[12].k,pos[15].k,pos[18].k, pos[21].k,pos[24].k, cp.k, spread);
upd (pos[1].k,pos[4].k,pos[7].k, pos[10].k,pos[13].k,pos[16].k,pos[19].k, pos[22].k,pos[25].k, cp.k, spread);
upd (pos[2].k,pos[5].k,pos[8].k, pos[11].k,pos[14].k,pos[17].k,pos[20].k, pos[23].k,pos[26].k, cp.k, spread);
}
bool History::GetSimilar(string &strStr, int LastCmdPartLength, bool bAppend)
{
int Length=(int)strStr.size();
if (LastCmdPartLength!=-1 && LastCmdPartLength<Length)
Length=LastCmdPartLength;
if (LastCmdPartLength==-1)
{
ResetPosition();
}
int i=0;
string strName;
unsigned __int64 HistoryItem=HistoryCfgRef()->CyclicGetPrev(m_TypeHistory, m_HistoryName, m_CurrentItem, strName);
while (HistoryItem != m_CurrentItem)
{
if (!HistoryItem)
{
if (++i > 1) //infinite loop
break;
HistoryItem = HistoryCfgRef()->CyclicGetPrev(m_TypeHistory, m_HistoryName, HistoryItem, strName);
continue;
}
if (!StrCmpNI(strStr.data(),strName.data(),Length) && strStr != strName)
{
if (bAppend)
strStr += strName.data() + Length;
else
strStr = strName;
m_CurrentItem = HistoryItem;
return true;
}
HistoryItem = HistoryCfgRef()->CyclicGetPrev(m_TypeHistory, m_HistoryName, HistoryItem, strName);
}
return false;
}
/*--------------------------------------------------------------------------------*/
void AudioObjectParameters::InitialiseToDefaults()
{
// reset all values to zero
memset(&values, 0, sizeof(values));
// reset set bitmap
setbitmap = 0;
// explicitly reset those parameters whose reset values are not zero
ResetPosition();
ResetGain();
ResetObjectImportance();
ResetChannelImportance();
ResetInterpolate();
ResetOtherValues();
// delete min and max position
ResetMinPosition();
ResetMaxPosition();
// delete entire chain of excluded zones
ResetExcludedZones();
}
// Set inertial navigation aiding mode
void NavEKF2_core::setAidingMode()
{
// Save the previous status so we can detect when it has changed
PV_AidingModePrev = PV_AidingMode;
// Determine if we should change aiding mode
switch (PV_AidingMode) {
case AID_NONE: {
// Don't allow filter to start position or velocity aiding until the tilt and yaw alignment is complete
// and IMU gyro bias estimates have stabilised
bool filterIsStable = tiltAlignComplete && yawAlignComplete && checkGyroCalStatus();
// If GPS usage has been prohiited then we use flow aiding provided optical flow data is present
// GPS aiding is the preferred option unless excluded by the user
bool canUseGPS = ((frontend->_fusionModeGPS) != 3 && readyToUseGPS() && filterIsStable && !gpsInhibit);
bool canUseRangeBeacon = readyToUseRangeBeacon() && filterIsStable;
if(canUseGPS || canUseRangeBeacon) {
PV_AidingMode = AID_ABSOLUTE;
} else if (optFlowDataPresent() && filterIsStable) {
PV_AidingMode = AID_RELATIVE;
}
}
break;
case AID_RELATIVE: {
// Check if the optical flow sensor has timed out
bool flowSensorTimeout = ((imuSampleTime_ms - flowValidMeaTime_ms) > 5000);
// Check if the fusion has timed out (flow measurements have been rejected for too long)
bool flowFusionTimeout = ((imuSampleTime_ms - prevFlowFuseTime_ms) > 5000);
// Enable switch to absolute position mode if GPS is available
// If GPS is not available and flow fusion has timed out, then fall-back to no-aiding
if((frontend->_fusionModeGPS) != 3 && readyToUseGPS() && !gpsInhibit) {
PV_AidingMode = AID_ABSOLUTE;
} else if (flowSensorTimeout || flowFusionTimeout) {
PV_AidingMode = AID_NONE;
}
}
break;
case AID_ABSOLUTE: {
// Find the minimum time without data required to trigger any check
uint16_t minTestTime_ms = MIN(frontend->tiltDriftTimeMax_ms, MIN(frontend->posRetryTimeNoVel_ms,frontend->posRetryTimeUseVel_ms));
// Check if optical flow data is being used
bool optFlowUsed = (imuSampleTime_ms - prevFlowFuseTime_ms <= minTestTime_ms);
// Check if airspeed data is being used
bool airSpdUsed = (imuSampleTime_ms - lastTasPassTime_ms <= minTestTime_ms);
// Check if range beacon data is being used
bool rngBcnUsed = (imuSampleTime_ms - lastRngBcnPassTime_ms <= minTestTime_ms);
// Check if GPS is being used
bool gpsPosUsed = (imuSampleTime_ms - lastPosPassTime_ms <= minTestTime_ms);
bool gpsVelUsed = (imuSampleTime_ms - lastVelPassTime_ms <= minTestTime_ms);
// Check if attitude drift has been constrained by a measurement source
bool attAiding = gpsPosUsed || gpsVelUsed || optFlowUsed || airSpdUsed || rngBcnUsed;
// check if velocity drift has been constrained by a measurement source
bool velAiding = gpsVelUsed || airSpdUsed || optFlowUsed;
// check if position drift has been constrained by a measurement source
bool posAiding = gpsPosUsed || rngBcnUsed;
// Check if the loss of attitude aiding has become critical
bool attAidLossCritical = false;
if (!attAiding) {
attAidLossCritical = (imuSampleTime_ms - prevFlowFuseTime_ms > frontend->tiltDriftTimeMax_ms) &&
(imuSampleTime_ms - lastTasPassTime_ms > frontend->tiltDriftTimeMax_ms) &&
(imuSampleTime_ms - lastRngBcnPassTime_ms > frontend->tiltDriftTimeMax_ms) &&
(imuSampleTime_ms - lastPosPassTime_ms > frontend->tiltDriftTimeMax_ms) &&
(imuSampleTime_ms - lastVelPassTime_ms > frontend->tiltDriftTimeMax_ms);
}
// Check if the loss of position accuracy has become critical
bool posAidLossCritical = false;
if (!posAiding ) {
uint16_t maxLossTime_ms;
if (!velAiding) {
maxLossTime_ms = frontend->posRetryTimeNoVel_ms;
} else {
maxLossTime_ms = frontend->posRetryTimeUseVel_ms;
}
posAidLossCritical = (imuSampleTime_ms - lastRngBcnPassTime_ms > maxLossTime_ms) &&
(imuSampleTime_ms - lastPosPassTime_ms > maxLossTime_ms);
}
if (attAidLossCritical) {
// if the loss of attitude data is critical, then put the filter into a constant position mode
PV_AidingMode = AID_NONE;
posTimeout = true;
velTimeout = true;
rngBcnTimeout = true;
tasTimeout = true;
gpsNotAvailable = true;
} else if (posAidLossCritical) {
// if the loss of position is critical, declare all sources of position aiding as being timed out
posTimeout = true;
velTimeout = true;
rngBcnTimeout = true;
gpsNotAvailable = true;
}
}
break;
default:
break;
}
// check to see if we are starting or stopping aiding and set states and modes as required
if (PV_AidingMode != PV_AidingModePrev) {
// set various usage modes based on the condition when we start aiding. These are then held until aiding is stopped.
switch (PV_AidingMode) {
case AID_NONE:
// We have ceased aiding
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "EKF2 IMU%u has stopped aiding",(unsigned)imu_index);
// When not aiding, estimate orientation & height fusing synthetic constant position and zero velocity measurement to constrain tilt errors
posTimeout = true;
velTimeout = true;
// Reset the normalised innovation to avoid false failing bad fusion tests
velTestRatio = 0.0f;
posTestRatio = 0.0f;
// store the current position to be used to keep reporting the last known position
lastKnownPositionNE.x = stateStruct.position.x;
lastKnownPositionNE.y = stateStruct.position.y;
// initialise filtered altitude used to provide a takeoff reference to current baro on disarm
// this reduces the time required for the baro noise filter to settle before the filtered baro data can be used
meaHgtAtTakeOff = baroDataDelayed.hgt;
// reset the vertical position state to faster recover from baro errors experienced during touchdown
stateStruct.position.z = -meaHgtAtTakeOff;
break;
case AID_RELATIVE:
// We have commenced aiding, but GPS usage has been prohibited so use optical flow only
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF2 IMU%u is using optical flow",(unsigned)imu_index);
posTimeout = true;
velTimeout = true;
// Reset the last valid flow measurement time
flowValidMeaTime_ms = imuSampleTime_ms;
// Reset the last valid flow fusion time
prevFlowFuseTime_ms = imuSampleTime_ms;
break;
case AID_ABSOLUTE: {
bool canUseGPS = ((frontend->_fusionModeGPS) != 3 && readyToUseGPS() && !gpsInhibit);
bool canUseRangeBeacon = readyToUseRangeBeacon();
// We have commenced aiding and GPS usage is allowed
if (canUseGPS) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF2 IMU%u is using GPS",(unsigned)imu_index);
}
posTimeout = false;
velTimeout = false;
// We have commenced aiding and range beacon usage is allowed
if (canUseRangeBeacon) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF2 IMU%u is using range beacons",(unsigned)imu_index);
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF2 IMU%u initial pos NE = %3.1f,%3.1f (m)",(unsigned)imu_index,(double)receiverPos.x,(double)receiverPos.y);
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF2 IMU%u initial beacon pos D offset = %3.1f (m)",(unsigned)imu_index,(double)bcnPosOffset);
}
// reset the last fusion accepted times to prevent unwanted activation of timeout logic
lastPosPassTime_ms = imuSampleTime_ms;
lastVelPassTime_ms = imuSampleTime_ms;
lastRngBcnPassTime_ms = imuSampleTime_ms;
}
break;
default:
break;
}
// Always reset the position and velocity when changing mode
ResetVelocity();
ResetPosition();
}
}
// Set inertial navigation aiding mode
void NavEKF2_core::setAidingMode()
{
// Determine when to commence aiding for inertial navigation
// Save the previous status so we can detect when it has changed
prevIsAiding = isAiding;
// Don't allow filter to start position or velocity aiding until the tilt and yaw alignment is complete
bool filterIsStable = tiltAlignComplete && yawAlignComplete;
// If GPS useage has been prohiited then we use flow aiding provided optical flow data is present
bool useFlowAiding = (frontend._fusionModeGPS == 3) && optFlowDataPresent();
// Start aiding if we have a source of aiding data and the filter attitude algnment is complete
// Latch to on. Aiding can be turned off by setting both
isAiding = ((readyToUseGPS() || useFlowAiding) && filterIsStable) || isAiding;
// check to see if we are starting or stopping aiding and set states and modes as required
if (isAiding != prevIsAiding) {
// We have transitioned either into or out of aiding
// zero stored velocities used to do dead-reckoning
heldVelNE.zero();
// reset the flag that indicates takeoff for use by optical flow navigation
takeOffDetected = false;
// set various useage modes based on the condition when we start aiding. These are then held until aiding is stopped.
if (!isAiding) {
// We have ceased aiding
// When not aiding, estimate orientation & height fusing synthetic constant position and zero velocity measurement to constrain tilt errors
PV_AidingMode = AID_NONE;
posTimeout = true;
velTimeout = true;
// store the current position to be used to keep reporting the last known position
lastKnownPositionNE.x = stateStruct.position.x;
lastKnownPositionNE.y = stateStruct.position.y;
// initialise filtered altitude used to provide a takeoff reference to current baro on disarm
// this reduces the time required for the baro noise filter to settle before the filtered baro data can be used
meaHgtAtTakeOff = baroDataDelayed.hgt;
// reset the vertical position state to faster recover from baro errors experienced during touchdown
stateStruct.position.z = -meaHgtAtTakeOff;
} else if (frontend._fusionModeGPS == 3) {
// We have commenced aiding, but GPS useage has been prohibited so use optical flow only
hal.console->printf("EKF is using optical flow\n");
PV_AidingMode = AID_RELATIVE; // we have optical flow data and can estimate all vehicle states
posTimeout = true;
velTimeout = true;
// Reset the last valid flow measurement time
flowValidMeaTime_ms = imuSampleTime_ms;
// Reset the last valid flow fusion time
prevFlowFuseTime_ms = imuSampleTime_ms;
} else {
// We have commenced aiding and GPS useage is allowed
hal.console->printf("EKF is using GPS\n");
PV_AidingMode = AID_ABSOLUTE; // we have GPS data and can estimate all vehicle states
posTimeout = false;
velTimeout = false;
// we need to reset the GPS timers to prevent GPS timeout logic being invoked on entry into GPS aiding
// this is becasue the EKF can be interrupted for an arbitrary amount of time during vehicle arming checks
lastTimeGpsReceived_ms = imuSampleTime_ms;
secondLastGpsTime_ms = imuSampleTime_ms;
// reset the last valid position fix time to prevent unwanted activation of GPS glitch logic
lastPosPassTime_ms = imuSampleTime_ms;
}
// Reset all position, velocity and covariance
ResetVelocity();
ResetPosition();
CovarianceInit();
}
// Always turn aiding off when the vehicle is disarmed
if (!isAiding) {
PV_AidingMode = AID_NONE;
posTimeout = true;
velTimeout = true;
}
}
bool Lexer::GetToken(Token&obj)
{
string strBuf;
int id;
DataValue dv;
while (!isEOF())
{
skipWS();
/////////////
// Check for chr/strlit
//
if (nextChar() == '-')
bool b=true;
if (nextChar() == '\'' || nextChar() == '\"')
{
char ch = getChar();
strBuf = "";
while (!isEOF())
{
if (nextChar() == '\r' || nextChar() == '\n')
{
dv.SetStrData("Newline in constant.");
throw Token(Token::LEX_ERROR,dv);
}
if (nextChar() == ch)
{
getChar();
if (ch == '\'')
{
if (strBuf.length() != 1)
{
dv.SetStrData("Character literals must have exactly one character.");
throw Token(Token::LEX_ERROR,dv);
}
dv.SetCharData(strBuf[0]);
obj=Token(Token::LEX_CHRLIT,dv);
return true;
}
else
{
dv.SetStrData(strBuf);
obj=Token(Token::LEX_STRLIT,dv);
return true;
}
}
strBuf += getChar();
}
dv.SetStrData("EOF in constant.");
throw Token(Token::LEX_ERROR,dv);
}
/////////////
// Check for Operators, then keywords
//
StorePosition();
id = dfaOperators.GetString(strBuf);
if (id == 0)
{
ResetPosition();
id = dfaKeywords.GetString(strBuf);
if (id == 0)
{
/////////////
// Check for numbers/idents
//
while (!isEOF() && !isWS(nextChar()) && !dfaOperators.ValidFirst(nextChar()))
{
strBuf += toLower(getChar());
}
//if it's all numbers
if (strBuf.find_first_not_of("0123456789",0,10) == string::npos)
{
//read in anything, including dots (will cover floats and invalid idents)
while (!isEOF() && !isWS(nextChar()) && (nextChar() == '.' || !dfaOperators.ValidFirst(nextChar())))
{
strBuf += toLower(getChar());
}
}
bool found = false;
if (strBuf.find_first_not_of("0123456789.",0,11) == string::npos)
{
string::size_type off;
off = strBuf.find('.',0);
if (off == string::npos)
{
dv.SetIntData(atoi(strBuf.c_str()));
obj=Token(Token::LEX_INTLIT,dv);
return true;
}
else
{
if (strBuf.find('.',off+1) == string::npos)
{
dv.SetFloatData(atof(strBuf.c_str()));
obj=Token(Token::LEX_FLOLIT,dv);
return true;
}
}
}
//validate identifier
try
{
if (strBuf.length() > 20)
throw 2;
for (string::iterator it = strBuf.begin();it != strBuf.end();++it)
{
if (it == strBuf.begin())
{
if (!isAlpha(*it))
{
throw 0;
}
}
if (!isAlpha(*it) && *it != '_' && !isNum(*it))
{
throw 1;
}
}
} catch(int iError)
{
switch (iError)
{
case 0:
dv.SetStrData("Unrecognized lexeme ("+strBuf+"): identifiers must begin with a letter.");
throw Token(Token::LEX_ERROR,dv);
case 1:
dv.SetStrData("Unrecognized lexeme ("+strBuf+"): identifiers can only contain underscores and alphanumeric characters.");
throw Token(Token::LEX_ERROR,dv);
case 2:
dv.SetStrData("Unrecognized lexeme ("+strBuf+"): identifiers can be at most 20 characters long.");
throw Token(Token::LEX_ERROR,dv);
}
}
dv.SetStrData(strBuf);
obj=Token(Token::LEX_IDENT,dv);
return true;
}
}
if (id)
{
dv.Clear();
obj=Token((Token::TokenType)id,dv);
return true;
}
dv.SetStrData("Undefined error.");
throw Token(Token::LEX_ERROR,dv);
}
//int 0 means eof
dv.SetStrData("EOF");
obj=Token(Token::LEX_ERROR,dv);
return false;
}
void Delete(HistoryRecord* Item){HistoryList.Delete(Item); ResetPosition(); SaveHistory();}
// fuse selected position, velocity and height measurements
void NavEKF2_core::FuseVelPosNED()
{
// start performance timer
hal.util->perf_begin(_perf_FuseVelPosNED);
// health is set bad until test passed
velHealth = false;
posHealth = false;
hgtHealth = false;
// declare variables used to check measurement errors
Vector3f velInnov;
// declare variables used to control access to arrays
bool fuseData[6] = {false,false,false,false,false,false};
uint8_t stateIndex;
uint8_t obsIndex;
// declare variables used by state and covariance update calculations
float posErr;
Vector6 R_OBS; // Measurement variances used for fusion
Vector6 R_OBS_DATA_CHECKS; // Measurement variances used for data checks only
Vector6 observation;
float SK;
// perform sequential fusion of GPS measurements. This assumes that the
// errors in the different velocity and position components are
// uncorrelated which is not true, however in the absence of covariance
// data from the GPS receiver it is the only assumption we can make
// so we might as well take advantage of the computational efficiencies
// associated with sequential fusion
if (fuseVelData || fusePosData || fuseHgtData) {
// set the GPS data timeout depending on whether airspeed data is present
uint32_t gpsRetryTime;
if (useAirspeed()) gpsRetryTime = frontend->gpsRetryTimeUseTAS_ms;
else gpsRetryTime = frontend->gpsRetryTimeNoTAS_ms;
// form the observation vector
observation[0] = gpsDataDelayed.vel.x;
observation[1] = gpsDataDelayed.vel.y;
observation[2] = gpsDataDelayed.vel.z;
observation[3] = gpsDataDelayed.pos.x;
observation[4] = gpsDataDelayed.pos.y;
observation[5] = -hgtMea;
// calculate additional error in GPS position caused by manoeuvring
posErr = frontend->gpsPosVarAccScale * accNavMag;
// estimate the GPS Velocity, GPS horiz position and height measurement variances.
// if the GPS is able to report a speed error, we use it to adjust the observation noise for GPS velocity
// otherwise we scale it using manoeuvre acceleration
// Use different errors if flying without GPS using synthetic position and velocity data
if (PV_AidingMode == AID_NONE && inFlight) {
// Assume the vehicle will be flown with velocity changes less than 10 m/s in this mode (realistic for indoor use)
// This is a compromise between corrections for gyro errors and reducing angular errors due to maneouvres
R_OBS[0] = sq(10.0f);
R_OBS[1] = R_OBS[0];
R_OBS[2] = R_OBS[0];
// Assume a large position uncertainty so as to contrain position states in this mode but minimise angular errors due to manoeuvres
R_OBS[3] = sq(25.0f);
R_OBS[4] = R_OBS[3];
} else {
if (gpsSpdAccuracy > 0.0f) {
// use GPS receivers reported speed accuracy if available and floor at value set by gps noise parameter
R_OBS[0] = sq(constrain_float(gpsSpdAccuracy, frontend->_gpsHorizVelNoise, 50.0f));
R_OBS[2] = sq(constrain_float(gpsSpdAccuracy, frontend->_gpsVertVelNoise, 50.0f));
} else {
// calculate additional error in GPS velocity caused by manoeuvring
R_OBS[0] = sq(constrain_float(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag);
R_OBS[2] = sq(constrain_float(frontend->_gpsVertVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsDVelVarAccScale * accNavMag);
}
R_OBS[1] = R_OBS[0];
R_OBS[3] = sq(constrain_float(frontend->_gpsHorizPosNoise, 0.1f, 10.0f)) + sq(posErr);
R_OBS[4] = R_OBS[3];
}
R_OBS[5] = posDownObsNoise;
// For data integrity checks we use the same measurement variances as used to calculate the Kalman gains for all measurements except GPS horizontal velocity
// For horizontal GPs velocity we don't want the acceptance radius to increase with reported GPS accuracy so we use a value based on best GPs perfomrance
// plus a margin for manoeuvres. It is better to reject GPS horizontal velocity errors early
for (uint8_t i=0; i<=1; i++) R_OBS_DATA_CHECKS[i] = sq(constrain_float(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag);
for (uint8_t i=2; i<=5; i++) R_OBS_DATA_CHECKS[i] = R_OBS[i];
// if vertical GPS velocity data and an independant height source is being used, check to see if the GPS vertical velocity and altimeter
// innovations have the same sign and are outside limits. If so, then it is likely aliasing is affecting
// the accelerometers and we should disable the GPS and barometer innovation consistency checks.
if (useGpsVertVel && fuseVelData && (frontend->_altSource != 2)) {
// calculate innovations for height and vertical GPS vel measurements
float hgtErr = stateStruct.position.z - observation[5];
float velDErr = stateStruct.velocity.z - observation[2];
// check if they are the same sign and both more than 3-sigma out of bounds
if ((hgtErr*velDErr > 0.0f) && (sq(hgtErr) > 9.0f * (P[8][8] + R_OBS_DATA_CHECKS[5])) && (sq(velDErr) > 9.0f * (P[5][5] + R_OBS_DATA_CHECKS[2]))) {
badIMUdata = true;
} else {
badIMUdata = false;
}
}
// calculate innovations and check GPS data validity using an innovation consistency check
// test position measurements
if (fusePosData) {
// test horizontal position measurements
innovVelPos[3] = stateStruct.position.x - observation[3];
innovVelPos[4] = stateStruct.position.y - observation[4];
varInnovVelPos[3] = P[6][6] + R_OBS_DATA_CHECKS[3];
varInnovVelPos[4] = P[7][7] + R_OBS_DATA_CHECKS[4];
// apply an innovation consistency threshold test, but don't fail if bad IMU data
float maxPosInnov2 = sq(max(0.01f * (float)frontend->_gpsPosInnovGate, 1.0f))*(varInnovVelPos[3] + varInnovVelPos[4]);
posTestRatio = (sq(innovVelPos[3]) + sq(innovVelPos[4])) / maxPosInnov2;
posHealth = ((posTestRatio < 1.0f) || badIMUdata);
// declare a timeout condition if we have been too long without data or not aiding
posTimeout = (((imuSampleTime_ms - lastPosPassTime_ms) > gpsRetryTime) || PV_AidingMode == AID_NONE);
// use position data if healthy, timed out, or in constant position mode
if (posHealth || posTimeout || (PV_AidingMode == AID_NONE)) {
posHealth = true;
// only reset the failed time and do glitch timeout checks if we are doing full aiding
if (PV_AidingMode == AID_ABSOLUTE) {
lastPosPassTime_ms = imuSampleTime_ms;
// if timed out or outside the specified uncertainty radius, reset to the GPS
if (posTimeout || ((P[6][6] + P[7][7]) > sq(float(frontend->_gpsGlitchRadiusMax)))) {
// reset the position to the current GPS position
ResetPosition();
// reset the velocity to the GPS velocity
ResetVelocity();
// don't fuse GPS data on this time step
fusePosData = false;
fuseVelData = false;
// Reset the position variances and corresponding covariances to a value that will pass the checks
zeroRows(P,6,7);
zeroCols(P,6,7);
P[6][6] = sq(float(0.5f*frontend->_gpsGlitchRadiusMax));
P[7][7] = P[6][6];
// Reset the normalised innovation to avoid failing the bad fusion tests
posTestRatio = 0.0f;
velTestRatio = 0.0f;
}
}
} else {
posHealth = false;
}
}
// test velocity measurements
if (fuseVelData) {
// test velocity measurements
uint8_t imax = 2;
// Don't fuse vertical velocity observations if inhibited by the user or if we are using synthetic data
if (frontend->_fusionModeGPS >= 1 || PV_AidingMode != AID_ABSOLUTE) {
imax = 1;
}
float innovVelSumSq = 0; // sum of squares of velocity innovations
float varVelSum = 0; // sum of velocity innovation variances
for (uint8_t i = 0; i<=imax; i++) {
// velocity states start at index 3
stateIndex = i + 3;
// calculate innovations using blended and single IMU predicted states
velInnov[i] = stateStruct.velocity[i] - observation[i]; // blended
// calculate innovation variance
varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS_DATA_CHECKS[i];
// sum the innovation and innovation variances
innovVelSumSq += sq(velInnov[i]);
varVelSum += varInnovVelPos[i];
}
// apply an innovation consistency threshold test, but don't fail if bad IMU data
// calculate the test ratio
velTestRatio = innovVelSumSq / (varVelSum * sq(max(0.01f * (float)frontend->_gpsVelInnovGate, 1.0f)));
// fail if the ratio is greater than 1
velHealth = ((velTestRatio < 1.0f) || badIMUdata);
// declare a timeout if we have not fused velocity data for too long or not aiding
velTimeout = (((imuSampleTime_ms - lastVelPassTime_ms) > gpsRetryTime) || PV_AidingMode == AID_NONE);
// use velocity data if healthy, timed out, or in constant position mode
if (velHealth || velTimeout) {
velHealth = true;
// restart the timeout count
lastVelPassTime_ms = imuSampleTime_ms;
// If we are doing full aiding and velocity fusion times out, reset to the GPS velocity
if (PV_AidingMode == AID_ABSOLUTE && velTimeout) {
// reset the velocity to the GPS velocity
ResetVelocity();
// don't fuse GPS velocity data on this time step
fuseVelData = false;
// Reset the normalised innovation to avoid failing the bad fusion tests
velTestRatio = 0.0f;
}
} else {
velHealth = false;
}
}
// test height measurements
if (fuseHgtData) {
// calculate height innovations
innovVelPos[5] = stateStruct.position.z - observation[5];
varInnovVelPos[5] = P[8][8] + R_OBS_DATA_CHECKS[5];
// calculate the innovation consistency test ratio
hgtTestRatio = sq(innovVelPos[5]) / (sq(max(0.01f * (float)frontend->_hgtInnovGate, 1.0f)) * varInnovVelPos[5]);
// fail if the ratio is > 1, but don't fail if bad IMU data
hgtHealth = ((hgtTestRatio < 1.0f) || badIMUdata);
// Fuse height data if healthy or timed out or in constant position mode
if (hgtHealth || hgtTimeout || (PV_AidingMode == AID_NONE && onGround)) {
// Calculate a filtered value to be used by pre-flight health checks
// We need to filter because wind gusts can generate significant baro noise and we want to be able to detect bias errors in the inertial solution
if (onGround) {
float dtBaro = (imuSampleTime_ms - lastHgtPassTime_ms)*1.0e-3f;
const float hgtInnovFiltTC = 2.0f;
float alpha = constrain_float(dtBaro/(dtBaro+hgtInnovFiltTC),0.0f,1.0f);
hgtInnovFiltState += (innovVelPos[5]-hgtInnovFiltState)*alpha;
} else {
hgtInnovFiltState = 0.0f;
}
// if timed out, reset the height
if (hgtTimeout) {
ResetHeight();
hgtTimeout = false;
}
// If we have got this far then declare the height data as healthy and reset the timeout counter
hgtHealth = true;
lastHgtPassTime_ms = imuSampleTime_ms;
}
}
// set range for sequential fusion of velocity and position measurements depending on which data is available and its health
if (fuseVelData && velHealth) {
fuseData[0] = true;
fuseData[1] = true;
if (useGpsVertVel) {
fuseData[2] = true;
}
tiltErrVec.zero();
}
if (fusePosData && posHealth) {
fuseData[3] = true;
fuseData[4] = true;
tiltErrVec.zero();
}
if (fuseHgtData && hgtHealth) {
fuseData[5] = true;
}
// fuse measurements sequentially
for (obsIndex=0; obsIndex<=5; obsIndex++) {
if (fuseData[obsIndex]) {
stateIndex = 3 + obsIndex;
// calculate the measurement innovation, using states from a different time coordinate if fusing height data
// adjust scaling on GPS measurement noise variances if not enough satellites
if (obsIndex <= 2)
{
innovVelPos[obsIndex] = stateStruct.velocity[obsIndex] - observation[obsIndex];
R_OBS[obsIndex] *= sq(gpsNoiseScaler);
}
else if (obsIndex == 3 || obsIndex == 4) {
innovVelPos[obsIndex] = stateStruct.position[obsIndex-3] - observation[obsIndex];
R_OBS[obsIndex] *= sq(gpsNoiseScaler);
} else if (obsIndex == 5) {
innovVelPos[obsIndex] = stateStruct.position[obsIndex-3] - observation[obsIndex];
const float gndMaxBaroErr = 4.0f;
const float gndBaroInnovFloor = -0.5f;
if(getTouchdownExpected()) {
// when a touchdown is expected, floor the barometer innovation at gndBaroInnovFloor
// constrain the correction between 0 and gndBaroInnovFloor+gndMaxBaroErr
// this function looks like this:
// |/
//---------|---------
// ____/|
// / |
// / |
innovVelPos[5] += constrain_float(-innovVelPos[5]+gndBaroInnovFloor, 0.0f, gndBaroInnovFloor+gndMaxBaroErr);
}
}
// calculate the Kalman gain and calculate innovation variances
varInnovVelPos[obsIndex] = P[stateIndex][stateIndex] + R_OBS[obsIndex];
SK = 1.0f/varInnovVelPos[obsIndex];
for (uint8_t i= 0; i<=15; i++) {
Kfusion[i] = P[i][stateIndex]*SK;
}
// inhibit magnetic field state estimation by setting Kalman gains to zero
if (!inhibitMagStates) {
for (uint8_t i = 16; i<=21; i++) {
Kfusion[i] = P[i][stateIndex]*SK;
}
} else {
for (uint8_t i = 16; i<=21; i++) {
Kfusion[i] = 0.0f;
}
}
// inhibit wind state estimation by setting Kalman gains to zero
if (!inhibitWindStates) {
Kfusion[22] = P[22][stateIndex]*SK;
Kfusion[23] = P[23][stateIndex]*SK;
} else {
Kfusion[22] = 0.0f;
Kfusion[23] = 0.0f;
}
// zero the attitude error state - by definition it is assumed to be zero before each observaton fusion
stateStruct.angErr.zero();
// calculate state corrections and re-normalise the quaternions for states predicted using the blended IMU data
for (uint8_t i = 0; i<=stateIndexLim; i++) {
statesArray[i] = statesArray[i] - Kfusion[i] * innovVelPos[obsIndex];
}
// the first 3 states represent the angular misalignment vector. This is
// is used to correct the estimated quaternion
stateStruct.quat.rotate(stateStruct.angErr);
// sum the attitude error from velocity and position fusion only
// used as a metric for convergence monitoring
if (obsIndex != 5) {
tiltErrVec += stateStruct.angErr;
}
// update the covariance - take advantage of direct observation of a single state at index = stateIndex to reduce computations
// this is a numerically optimised implementation of standard equation P = (I - K*H)*P;
for (uint8_t i= 0; i<=stateIndexLim; i++) {
for (uint8_t j= 0; j<=stateIndexLim; j++)
{
KHP[i][j] = Kfusion[i] * P[stateIndex][j];
}
}
for (uint8_t i= 0; i<=stateIndexLim; i++) {
for (uint8_t j= 0; j<=stateIndexLim; j++) {
P[i][j] = P[i][j] - KHP[i][j];
}
}
}
}
}
// force the covariance matrix to be symmetrical and limit the variances to prevent ill-condiioning.
ForceSymmetry();
ConstrainVariances();
// stop performance timer
hal.util->perf_end(_perf_FuseVelPosNED);
}
void FSimpleHMD::ResetOrientationAndPosition(float yaw)
{
ResetOrientation(yaw);
ResetPosition();
}
// select fusion of optical flow measurements
void NavEKF2_core::SelectFlowFusion()
{
// Check if the magnetometer has been fused on that time step and the filter is running at faster than 200 Hz
// If so, don't fuse measurements on this time step to reduce frame over-runs
// Only allow one time slip to prevent high rate magnetometer data preventing fusion of other measurements
if (magFusePerformed && dtIMUavg < 0.005f && !optFlowFusionDelayed) {
optFlowFusionDelayed = true;
return;
} else {
optFlowFusionDelayed = false;
}
// start performance timer
hal.util->perf_begin(_perf_FuseOptFlow);
// Perform Data Checks
// Check if the optical flow data is still valid
flowDataValid = ((imuSampleTime_ms - flowValidMeaTime_ms) < 1000);
// Check if the optical flow sensor has timed out
bool flowSensorTimeout = ((imuSampleTime_ms - flowValidMeaTime_ms) > 5000);
// Check if the fusion has timed out (flow measurements have been rejected for too long)
bool flowFusionTimeout = ((imuSampleTime_ms - prevFlowFuseTime_ms) > 5000);
// check is the terrain offset estimate is still valid
gndOffsetValid = ((imuSampleTime_ms - gndHgtValidTime_ms) < 5000);
// Perform tilt check
bool tiltOK = (Tnb_flow.c.z > frontend.DCM33FlowMin);
// Constrain measurements to zero if we are using optical flow and are on the ground
if (frontend._fusionModeGPS == 3 && !takeOffDetected && isAiding) {
ofDataDelayed.flowRadXYcomp.zero();
ofDataDelayed.flowRadXY.zero();
flowDataValid = true;
}
// If the flow measurements have been rejected for too long and we are relying on them, then revert to constant position mode
if ((flowSensorTimeout || flowFusionTimeout) && PV_AidingMode == AID_RELATIVE) {
PV_AidingMode = AID_NONE;
// reset the velocity
ResetVelocity();
// store the current position to be used to as a sythetic position measurement
lastKnownPositionNE.x = stateStruct.position.x;
lastKnownPositionNE.y = stateStruct.position.y;
// reset the position
ResetPosition();
}
// if we do have valid flow measurements, fuse data into a 1-state EKF to estimate terrain height
// we don't do terrain height estimation in optical flow only mode as the ground becomes our zero height reference
if ((newDataFlow || newDataRng) && tiltOK) {
// fuse range data into the terrain estimator if available
fuseRngData = newDataRng;
// fuse optical flow data into the terrain estimator if available and if there is no range data (range data is better)
fuseOptFlowData = (newDataFlow && !fuseRngData);
// Estimate the terrain offset (runs a one state EKF)
EstimateTerrainOffset();
// Indicate we have used the range data
newDataRng = false;
}
// Fuse optical flow data into the main filter if not excessively tilted and we are in the correct mode
if (newDataFlow && tiltOK && PV_AidingMode == AID_RELATIVE)
{
// Set the flow noise used by the fusion processes
R_LOS = sq(max(frontend._flowNoise, 0.05f));
// ensure that the covariance prediction is up to date before fusing data
if (!covPredStep) CovariancePrediction();
// Fuse the optical flow X and Y axis data into the main filter sequentially
FuseOptFlow();
// reset flag to indicate that no new flow data is available for fusion
newDataFlow = false;
}
// stop the performance timer
hal.util->perf_end(_perf_FuseOptFlow);
}
static int OpenLWObject(char *name, LWOBInfo *object)
{
char temp[10];
int readsize=0;
int objsize;
int polsize, pntsize;
int tempsize;
object->fp=NULL;
object->pntPos=0;
object->polPos=0;
object->polSize=0;
object->pntSize=0;
object->nSdat = 0; /* [EW 7 Jun 00] */
object->fp=fopen(name,"rb");
if (object->fp==NULL)
return(0);
fseek(object->fp, 8, SEEK_SET);
fread(&temp,4,1,object->fp);
if (strncmp(temp,"LWOB",4)==0)
{
//Get the object size
fseek(object->fp,4,SEEK_SET);
fread(&objsize,4,1,object->fp);
BSWAP_L(objsize);
fseek(object->fp,4,SEEK_CUR); //Skip the LWOB deal
readsize=4;
while(readsize<objsize){
fread(temp,4,1,object->fp);
if(strncmp(temp,"POLS",4)==0){
fread(&polsize,4,1,object->fp);
BSWAP_L(polsize);
fgetpos(object->fp,&object->polPos);
fseek(object->fp,polsize,SEEK_CUR);
object->polSize=polsize;
readsize+=polsize+8;
}
else if(strncmp(temp,"PNTS",4)==0){
fread(&pntsize,4,1,object->fp);
BSWAP_L(pntsize);
fgetpos(object->fp,&object->pntPos);
fseek(object->fp,pntsize,SEEK_CUR);
object->pntSize=pntsize;
readsize+=pntsize+8;
}
else if(strncmp(temp,"SRFS",4)==0){
fread(&pntsize,4,1,object->fp);
BSWAP_L(pntsize);
fgetpos(object->fp,&object->srfPos);
fseek(object->fp,pntsize,SEEK_CUR);
object->srfSize=pntsize;
readsize+=pntsize+8;
}
else{
if(strncmp(temp,"SURF",4)==0)
object->nSdat++;
fread(&tempsize,4,1,object->fp);
BSWAP_L(tempsize);
fseek(object->fp,tempsize,SEEK_CUR);
readsize+=tempsize+8;
}
}
ResetPosition(object);
}
else return(0);
//printf("Not a LightWave Object!\n");
return(1);
}
// ----------------------------------------------------------------------------
// Name : AdjustPosition()
// Desc :
// ----------------------------------------------------------------------------
void CUICreateChar::AdjustPosition( PIX pixMinI, PIX pixMinJ, PIX pixMaxI, PIX pixMaxJ )
{
ResetPosition( pixMinI, pixMinJ, pixMaxI, pixMaxJ );
}
void FSteamVRHMD::ResetOrientationAndPosition(float yaw)
{
ResetOrientation(yaw);
ResetPosition();
}
// ----------------------------------------------------------------------------
// Name : AdjustPosition()
// Desc :
// ----------------------------------------------------------------------------
void CUIChildQuickSlot::AdjustPosition( PIX pixMinI, PIX pixMinJ, PIX pixMaxI, PIX pixMaxJ )
{
ResetPosition( pixMinI, pixMinJ, pixMaxI, pixMaxJ );
}
// ----------------------------------------------------------------------------
// Name : AdjustPosition()
// Desc :
// ----------------------------------------------------------------------------
void CUICompound::AdjustPosition( PIX pixMinI, PIX pixMinJ, PIX pixMaxI, PIX pixMaxJ )
{
if( m_nPosX < pixMinI || m_nPosX + GetWidth() > pixMaxI ||
m_nPosY < pixMinJ || m_nPosY + GetHeight() > pixMaxJ )
ResetPosition( pixMinI, pixMinJ, pixMaxI, pixMaxJ );
}
void CFloorPowerFunctions::PostStep(){
ResetPosition();
SetPovCamera();
}
static LWObject *createLWObject(const char *name)
{
char sname[MAX_SURFNAME];
LWOBInfo LWObj;
LWPoint pnt;
LWObject *Object=NULL;
LWSurface surf;
int p;
surf.name = sname;
surf.data = NULL;
surf.size = 0;
if(!OpenLWObject((char *)name,&LWObj))
return(NULL);
if(Object=malloc(sizeof(LWObject)+strlen(name)+2) )
{
memset(Object,0,sizeof(LWObject));
Object->name = (char *)&(Object[1]);
strcpy(Object->name,name);
while(GetPolygon(&LWObj,&MaxPoly)==1)
Object->nPols++;
ResetPosition(&LWObj);
if(Object->pols = calloc(sizeof(LWPolygon),Object->nPols))
{
for(p=0; (p<Object->nPols) && (GetPolygon(&LWObj,&MaxPoly)==1); p++ )
copyPolygon(&(Object->pols[p]), &MaxPoly);
}
while(GetPoint(&LWObj,&pnt)==1)
Object->nPnts++;
ResetPosition(&LWObj);
if(Object->pnts = calloc(sizeof(LWPoint),Object->nPnts))
{
double r;
for(p=0; (p<Object->nPnts) && (GetPoint(&LWObj,&pnt)==1); p++ )
{
if(p)
{
if(pnt.x>Object->bounds[BB_XMAX])
Object->bounds[BB_XMAX] = pnt.x;
if(pnt.x<Object->bounds[BB_XMIN])
Object->bounds[BB_XMIN] = pnt.x;
if(pnt.y>Object->bounds[BB_YMAX])
Object->bounds[BB_YMAX] = pnt.y;
if(pnt.y<Object->bounds[BB_YMIN])
Object->bounds[BB_YMIN] = pnt.y;
if(pnt.z>Object->bounds[BB_ZMAX])
Object->bounds[BB_ZMAX] = pnt.z;
if(pnt.z<Object->bounds[BB_ZMIN])
Object->bounds[BB_ZMIN] = pnt.z;
r = sqrt(pnt.x*pnt.x + pnt.y*pnt.y + pnt.z*pnt.z);
if(Object->rMax<r)
Object->rMax = r;
}
else // initialize max values = first values
{
Object->rMax = sqrt(pnt.x*pnt.x + pnt.y*pnt.y + pnt.z*pnt.z);
Object->bounds[BB_XMIN] = Object->bounds[BB_XMAX] = pnt.x;
Object->bounds[BB_YMIN] = Object->bounds[BB_YMAX] = pnt.y;
Object->bounds[BB_ZMIN] = Object->bounds[BB_ZMAX] = pnt.z;
}
Object->pnts[p] = pnt;
}
while(GetSurface(&LWObj,&surf)==1)
Object->nSurfs++;
if(Object->surfs = malloc( LWObj.srfSize+(sizeof(LWSurface)*(Object->nSurfs+1)) ))
{
int i,len,siz;
fpos_t datpos;
char *buf = (char *)&(Object->surfs[Object->nSurfs+1]);
memset(Object->surfs,0,( LWObj.srfSize + (sizeof(LWSurface)*(Object->nSurfs+1)) ));
GetSRF(&LWObj,buf,LWObj.srfSize);
for(i=0; i<Object->nSurfs; i++)
{
Object->surfs[i].name = buf;
Object->surfs[i].size = 0;
Object->surfs[i].data = NULL;
if( (siz=GetSDatSize(&LWObj,buf,&datpos)) )
{
if( (Object->surfs[i].data=malloc(siz)) )
{
GetSDat(&LWObj,Object->surfs[i].data,siz,&datpos);
Object->surfs[i].size = siz;
}
}
len = strlen(buf) + 1; // get NULL too
if(len&1) len++;
buf += len;
}
Object->surfs[i].name = NULL;
}
}
}
CloseLWObject(&LWObj);
#ifdef HEAP_PROBLEM
_heapmin();
#endif
return(Object);
}
