public void InitializeAvailableExpressions(TargetTypes targetType)
{
ConditionBlock.AvailableChildrenGetters.Clear();
switch (targetType)
{
case TargetTypes.Item:
//Items conditions menu items;
var availableItemElements = new Func<CompositeElement>[] {
()=> new ConditionEntryIs(this.ExpressionViewModel),
()=> new ConditionItemTypeIs(this.ExpressionViewModel),
()=> new ConditionItemPropertyIs(this.ExpressionViewModel),
()=> new ConditionItemsCategoryIs(this.ExpressionViewModel),
()=> new ConditionItemStoreIs(this.ExpressionViewModel),
};
ConditionBlock.WithAvailabeChildren(availableItemElements);
break;
case TargetTypes.Category:
//Category conditions menu items;
var availableCategoryElements = new Func<CompositeElement>[] {
()=> new ConditionCategoryIs(this.ExpressionViewModel),
()=> new ConditionCategoryPropertyIs(this.ExpressionViewModel),
()=> new ConditionCategoryIsSubcategory(this.ExpressionViewModel),
()=> new ConditionCategoryStoreIs(this.ExpressionViewModel),
};
ConditionBlock.WithAvailabeChildren(availableCategoryElements);
break;
}
ConditionBlock.NewChildLabel = "+ add condition".Localize(null, LocalizationScope.DefaultCategory);
}
public PossibleOOIMessage(int robotID, Vector2 point, double ts, TargetTypes s)
{
this.point = point;
this.timestamp = ts;
this.ooiType = s;
this.robotID = robotID;
}
public TargetListMessage(int robotID, RobotImage image, double timeStamp, int targetID, TargetTypes type)
{
this.robotID = robotID;
this.image = image;
this.timeStamp = timeStamp;
this.targetID = targetID;
this.type = type;
}
public void SetAttributes(string _nam, string _des, int _dam, TargetTypes _tar, TargetAttribute _atr)
{
_name = _nam;
_description = _des;
_damage = _dam;
_target = _tar;
_attribute = _atr;
}
public static List<BaseEnemy> FilterTargets(List<BaseEnemy> enemies, TargetTypes TargetType)
{
if (TargetType == TargetTypes.Ground)
enemies = enemies.Where(x => x.GetType() == typeof(GroundEnemy)).ToList();
if (TargetType == TargetTypes.Flying)
enemies = enemies.Where(x => x.GetType() == typeof(FlyingEnemy)).ToList();
return enemies;
}
public TargetListMessage(int robotID, RobotImage image, double timeStamp, int targetID, Vector2 targetPose, Matrix targetCov, TargetTypes type)
{
this.robotID = robotID;
this.image = image;
this.timeStamp = timeStamp;
this.targetID = targetID;
this.targetPose = targetPose;
this.type = type;
this.cov = targetCov;
}
protected AbstractActionAbility(String name, int actionCost, AbilityType abilityType, TargetTypes targetTypes,
DefaultTargetType defaultTarget, AnimationType animType , AbstractDamageBehaviour damageBehaviour)
: base(name, actionCost, abilityType)
{
this.TargetType = targetTypes;
this.DefaultTarget = defaultTarget;
abilityType |= AbilityType.Action;
//this.AnimationBehaviour = animBehaviour;
this.DamageBehaviour = damageBehaviour;
this.AnimationType = animType;
}
/// <summary>
/// Gets the display template.
/// </summary>
/// <param name="target">The target.</param>
/// <param name="tags">The tags.</param>
/// <returns></returns>
public string GetDisplayTemplate(TargetTypes target, TagSet tags)
{
var session = _customerSession.CustomerSession;
var tagsLocal = tags ?? session.GetCustomerTagSet();
return Helper.Get(
string.Format(DisplayTemplateCacheKey, target.ToString(), tagsLocal.GetCacheKey()),
() => _service.GetTemplate(target, tags),
AppConfigConfiguration.Instance.Cache.DisplayTemplateMappingsTimeout,
IsEnabled);
}
public DynamicTarget(Guid g)
{
if (RTCore.Instance.Network[g] != null) {
sat = RTCore.Instance.Network[g];
type = TargetTypes.ISATELLITE;
} else if (RTCore.Instance.Network.Planets.ContainsKey(g)) {
body = RTCore.Instance.Network.Planets[g];
type = TargetTypes.BODY;
} else {
type = TargetTypes.NOTARGET;
}
}
private void Start()
{
// Setup light and shadows
myLight = GetComponent <Light>();
myLight.enabled = EnableLight;
forceDisableSpellLighting = !DaggerfallUnity.Settings.EnableSpellLighting;
forceDisableSpellShadows = !DaggerfallUnity.Settings.EnableSpellShadows;
if (forceDisableSpellLighting)
{
myLight.enabled = false;
}
if (forceDisableSpellShadows)
{
myLight.shadows = LightShadows.None;
}
initialRange = myLight.range;
initialIntensity = myLight.intensity;
// Setup collider
myCollider = GetComponent <SphereCollider>();
myCollider.radius = ColliderRadius;
myCollider.isTrigger = true;
// Setup rigidbody
myRigidbody = GetComponent <Rigidbody>();
myRigidbody.isKinematic = true;
myRigidbody.useGravity = false;
// Use payload when available
if (payload != null)
{
// Set payload missile properties
caster = payload.CasterEntityBehaviour;
targetType = payload.Settings.TargetType;
elementType = payload.Settings.ElementType;
// Set spell billboard anims automatically from payload for mobile missiles
if (targetType == TargetTypes.SingleTargetAtRange ||
targetType == TargetTypes.AreaAtRange)
{
UseSpellBillboardAnims(elementType);
}
}
// Ignore missile collision with caster (this is a different check to AOE targets)
if (caster)
{
Physics.IgnoreCollision(caster.GetComponent <Collider>(), this.GetComponent <Collider>());
}
}
public override void CreateMap(IMapperConfigurationExpression configuration, Type type)
{
if (TargetTypes.IsNullOrEmpty())
{
return;
}
configuration.CreateAutoAttributeMaps(type, TargetTypes, MemberList.Source);
foreach (var targetType in TargetTypes)
{
configuration.CreateAutoAttributeMaps(targetType, new[] { type }, MemberList.Destination);
}
}
private string GetTargetTag(TargetTypes targetType)
{
switch (targetType)
{
case TargetTypes.Tree:
return(SimulationSettings.HealthyTreeTag);
case TargetTypes.Stockpile:
return(SimulationSettings.StockpileTag);
default:
return("");
}
}
public SelectShipFilter
(
int minRange,
int maxRange,
ArcType inArcType = ArcType.None,
TargetTypes targetTypes = TargetTypes.Any,
Type hasToken = null)
{
MinRange = minRange;
MaxRange = maxRange;
InArcType = inArcType;
TargetTypes = targetTypes;
HasToken = hasToken;
}
public void SetTarget(Unit target, TargetTypes targetType)
{
if (target == null)
{
Targets[(int)targetType] = 0;
CurrentTarget = null;
}
else
{
Targets[(int)targetType] = target.Oid;
CurrentTarget = target;
}
}
/// <summary>
///
/// </summary>
/// <param name="skillJSONParser">The JSON parser containing the skill's data.</param>
private void InitializeSkillValues(SkillJSONParser skillJSONParser)
{
nameID = skillJSONParser.GetNameID();
descriptionID = skillJSONParser.GetDescriptionID();
longDescriptionID = skillJSONParser.GetLongDescriptionID();
iconFilePath = skillJSONParser.GetIconFilePath();
encounterSkillType = skillJSONParser.GetEncounterSkillType();
targetType = skillJSONParser.GetTargetType();
aiSkillCategory = skillJSONParser.GetAISkillCategory();
costs = skillJSONParser.GetCosts();
cooldown = skillJSONParser.GetCooldown();
hits = skillJSONParser.GetHits();
skillChoreographies = skillJSONParser.GetSkillChoreographies();
skillFunctionName = skillJSONParser.GetSkillFunctionName();
}
public override void CreateMap(TypeAdapterConfig configuration, Type destination)
{
if (TargetTypes.IsNullOrEmpty())
{
return;
}
foreach (Type source in TargetTypes)
{
MethodInfo mapToDestination = configuration.GetType().GetMethod("NewConfig").MakeGenericMethod(source, destination);
MethodInfo mapToSource = configuration.GetType().GetMethod("NewConfig").MakeGenericMethod(destination, source);
mapToDestination.Invoke(configuration, null);
mapToSource.Invoke(configuration, null);
}
}
public bool IsTypeMatched(Type type)
{
if (TargetTypes == null)
{
return(false);
}
if (IncludeInherited)
{
return(TargetTypes.Any(target => type == target || type.IsSubclassOf(target)));
}
else
{
return(TargetTypes.Any(target => target == type));
}
}
/// <summary>
/// Enqueue a target.
/// </summary>
/// <param name="entityId">Target's ID</param>
/// <param name="priority">Priority of the target, lower number is higher priority</param>
/// <param name="subPriority">Sub-priority for sorting</param>
/// <param name="type">Type of the target, determines what handles it</param>
public void EnqueueTarget(Int64 entityId, int priority, int subPriority, TargetTypes type)
{
var methodName = "EnqueueTarget";
LogTrace(methodName, "{0},{1},{2},{3}", entityId, priority, subPriority, type);
if (!IsQueued(entityId))
{
LogMessage(methodName, LogSeverityTypes.Debug, "Queueing entity with id {0}, priority {1}, sub priority {2}, type {3}",
entityId, priority, subPriority, type);
_targets.Add(new QueueTarget(entityId, priority, subPriority, type));
}
else
{
var existingTarget = _targets.FirstOrDefault(queueTarget => queueTarget.Id == entityId);
if (existingTarget == null)
{
return;
}
var shouldUpdate = false;
if (priority < existingTarget.Priority)
{
LogMessage(methodName, LogSeverityTypes.Debug, "Queue target {0} has increased in priority to {1}.",
existingTarget.Id, priority);
shouldUpdate = true;
}
else if (priority == existingTarget.Priority && subPriority < existingTarget.SubPriority)
{
LogMessage(methodName, LogSeverityTypes.Debug, "Queue target {0} has increased in subpriority to {1}.",
existingTarget.Id, subPriority);
shouldUpdate = true;
}
else if (type != existingTarget.Type)
{
LogMessage(methodName, LogSeverityTypes.Debug, "Queue target {0} has changed type to {1}.",
existingTarget.Id, type);
shouldUpdate = true;
}
if (shouldUpdate)
{
existingTarget.UpdateTarget(priority, subPriority, type);
}
}
}
public override Unit GetTarget(TargetTypes type)
{
if (_Owner?.Region == null)
{
return(null);
}
Unit u = null;
ushort oid = Targets[(int)type];
if (oid != 0)
{
u = _Owner.Region.GetObject(oid) as Unit;
}
return(u);
}
public override void CreateMap(Type type)
{
if (TargetTypes.IsNullOrEmpty())
{
return;
}
foreach (var targetType in TargetTypes)
{
//if (targetType.FullName == "WinMS.Core.Entities.Info.BarcodeInfo")
//{
// var ww = "";
//}
Nelibur.ObjectMapper.TinyMapper.Bind(type, targetType);
Nelibur.ObjectMapper.TinyMapper.Bind(targetType, type);
}
}
private GameObject SearchForNearby(TargetTypes targetType)
{
string targetTag = GetTargetTag(targetType);
GameObject[] candidateTargets = GameObject.FindGameObjectsWithTag(targetTag);
if (targetType == TargetTypes.Stockpile)
{
candidateTargets = FilterForSameTeam(candidateTargets);
}
if (candidateTargets.Length > 0)
{
return(ChooseNearest(candidateTargets));
}
return(null);
}
public SelectShipFilter
(
int minRange = 0,
int maxRange = int.MaxValue,
ArcType inArcType = ArcType.None,
TargetTypes targetTypes = TargetTypes.Any,
Type hasToken = null,
List <Type> shipTypesOnly = null
)
{
MinRange = minRange;
MaxRange = maxRange;
InArcType = inArcType;
TargetTypes = targetTypes;
HasToken = hasToken;
ShipTypesOnly = shipTypesOnly;
}
public static string EndPoint(TargetTypes type, SubTargetType subType, string param)
{
string targetType = EnumMapper.MapTarget(type);
string subTarget = EnumMapper.Map(subType);
if (targetType != null)
{
if (subType != SubTargetType.not_applicable)
{
return(string.Format("https://api-nba-v1.p.rapidapi.com/{0}/{1}/{2}", targetType, subTarget, param));
}
return(string.Format("https://api-nba-v1.p.rapidapi.com/{0}/", targetType));
}
return(string.Empty);
}
public DynamicTarget(Guid g)
{
if (RTCore.Instance.Network[g] != null)
{
sat = RTCore.Instance.Network[g];
type = TargetTypes.ISATELLITE;
}
else if (RTCore.Instance.Network.Planets.ContainsKey(g))
{
body = RTCore.Instance.Network.Planets[g];
type = TargetTypes.BODY;
}
else
{
type = TargetTypes.NOTARGET;
}
}
public Repulsor()
{
this.Name = "Repulsor";
this.Width = 3;
this.Height = 2;
this.Origin = new Point16(1, 1);
this.Settings.Add("TargetType", TargetTypes.First());
this.RightClickHelp = $"Right Click to change targetting type ({string.Join("/", TargetTypes)})";
this.PinLayout = new List <PinDesign>
{
new PinDesign("In", 0, new Point16(1, 0), "bool", "Armed"),
new PinDesign("In", 1, new Point16(0, 1), "int", "Velocity"),
new PinDesign("In", 2, new Point16(2, 1), "int", "Distance"),
};
}
private void StartMovingTowardsEntity(TargetTypes targetType, GameObject target)
{
switch (targetType)
{
case TargetTypes.Tree:
lumberjack.Send(new NPCLumberjack.Update().SetCurrentState(LumberjackFSMState.StateEnum.MOVING_TO_TREE).SetTargetEntity(target.EntityId()));
break;
case TargetTypes.Stockpile:
lumberjack.Send(new NPCLumberjack.Update().SetCurrentState(LumberjackFSMState.StateEnum.MOVING_TO_STOCKPILE).SetTargetEntity(target.EntityId()));
break;
default:
Debug.LogError("I'm moving towards something which isn't a tree or stockpile, help...");
break;
}
navigation.SetNavigation(target.transform.position.ToCoordinates());
}
public override bool CanTarget(GridOccupant target, GridOccupant wielder, Grid.Grid grid)
{
var direction = target.Position - wielder.Position;
if (!direction.IsCardinal() || direction.CardinalMagnitude() > 1)
{
return(false);
}
var node = new Grid.Grid.Node();
if (grid.TryGetNodeAt(wielder.Position, ref node))
{
return(node.Occupants.Any(it => TargetTypes.Contains(it.Type)));
}
return(false);
}
private Enemy GetTargetByDistance(TargetTypes distance)
{
List <Enemy> enemies = GetEnemiesInRange();
List <float> distances = new List <float>();
for (int i = 0; i < enemies.Count; i++)
{
if (distance == TargetTypes.CLOSEST)
{
distances.Add(Vector3.Distance(transform.position, enemies[i].transform.position));
}
else if (distance == TargetTypes.FURTHEST)
{
distances.Add(enemies[i].WaypointIndex);
}
}
float minValue = distances.Min();
float maxValue = distances.Max();
switch (distance)
{
case TargetTypes.CLOSEST:
for (int i = 0; i < distances.Count; i++)
{
if (distances[i] == minValue)
{
return(enemies[i]);
}
}
break;
case TargetTypes.FURTHEST:
for (int i = 0; i < distances.Count; i++)
{
if (distances[i] == maxValue)
{
return(enemies[i]);
}
}
break;
}
return(null);
}
protected override void Initialize(LSEventArgs copy)
{
var methodName = "Initialize";
base.Initialize(copy);
Int64 targetEntityId;
int targetPriority, targetSubPriority;
DateTime targetTimeQueued;
TargetTypes targetType;
if (copy.Args.Length < 7)
{
_logging.LogMessage(ObjectName, methodName, LogSeverityTypes.Standard, "Args array is is missing elements.");
return;
}
if (!Int64.TryParse(copy.Args[2], out targetEntityId))
{
_logging.LogMessage(ObjectName, methodName, LogSeverityTypes.Standard, "Unable to parse target entity ID {0}", copy.Args[2]);
}
if (!int.TryParse(copy.Args[3], out targetPriority))
{
_logging.LogMessage(ObjectName, methodName, LogSeverityTypes.Standard, "Unable to parse target priority {0}", copy.Args[3]);
}
if (!int.TryParse(copy.Args[4], out targetSubPriority))
{
_logging.LogMessage(ObjectName, methodName, LogSeverityTypes.Standard, "Unable to parse target sub priority {0}", copy.Args[4]);
}
if (!DateTime.TryParse(copy.Args[5], out targetTimeQueued))
{
_logging.LogMessage(ObjectName, methodName, LogSeverityTypes.Standard, "Unable to parse target time queued {0}", copy.Args[5]);
}
if (!TargetTypes.TryParse(copy.Args[6], out targetType))
{
_logging.LogMessage(ObjectName, methodName, LogSeverityTypes.Standard, "Unable to parse target type {0}", copy.Args[6]);
}
Target = new QueueTarget(targetEntityId, targetPriority, targetSubPriority, targetType);
}
private void SearchNextCheckpoint()
{
_distanceTarget = null;
var checkpoints = (List <Checkpoint>)_gameManager.Checkpoints;
for (int i = _distanceTargetIndex; i < checkpoints.Count; i++)
{
int index = _basicComponent.Team.TeamNo == 1 ? i : checkpoints.Count - 1 - i;
if (!checkpoints[index].CheckType(_basicComponent, Checkpoint.Type.FRIENDLY))
{
_distanceTarget = checkpoints[index].transform;
_distanceTargetType = TargetTypes.CHECKPOINT;
_distanceTargetIndex = i;
break;
}
}
}
/// <summary>
/// Creates a new unit instance from a JSON data file.
/// </summary>
/// <param name="unitJSONFileName">The name of the JSON file, relative to the
/// "Assets/Resources/JSON Assets/Units/" directory.</param>
/// <param name="level">The unit's level.</param>
/// <param name="targetType">The unit's targeting type.</param>
public Unit(string unitJSONFileName, int level, TargetTypes targetType) : base()
{
UnitJSONParser unitJSONParser = new UnitJSONParser(unitJSONFileName);
GameObject g = (GameObject)Resources.Load("Prefabs/Overworld Object");
GameObject overworldObject = GameObject.Instantiate(g);
overworldObjectCoordinator = overworldObject.GetComponent <OverworldObjectCoordinator>();
overworldObjectCoordinator.overworldObject = this;
this.level = level;
this.targetType = targetType;
InitializeUnitBaseValues(unitJSONParser);
InitializeStats();
InitializeUnitFunctionality(unitJSONParser);
CalculateFrameSpeeds(15, 5, 5);
}
private void SearchNextWaypoint()
{
if (_basicComponent.Team == null || _basicComponent.Team.TeamNo == GameManager.MOBTEAMNO)
{
return;
}
var w = _gameManager.CurrentSection.transform.GetComponentsInChildren <Waypoint>().FirstOrDefault(
waypoint =>
/*waypoint.TeamNo == _basicComponent.Team.TeamNo &&*/
((_distanceTarget == null && waypoint.Sort == 0) ||
(_distanceTargetIndex + 1 == waypoint.Sort)));
if (w != null)
{
_distanceTarget = w.transform;
_distanceTargetType = TargetTypes.WAYPOINT;
}
}
void Awake()
{
cam = Camera.main;
if (target == TargetTypes.Tag)
{
GameObject go = GameObject.FindWithTag(targetTag);
if (go != null)
{
targetTransform = go.transform;
target = TargetTypes.GameObject;
}
else
{
Debug.LogError("[Follower2D] Tag " + targetTag + " not found in scene");
}
}
}
public override void CreateMap(IMapperConfigurationExpression configuration, Type type)
{
if (TargetTypes.IsNullOrEmpty())
{
return;
}
foreach (var targetType in TargetTypes)
{
var configMap = configuration.CreateMap(targetType, type, MemberList.Destination);
if (selfMemberOptions != null)
{
foreach (var selfMemberOption in selfMemberOptions)
{
configMap.ForMember(selfMemberOption.Key, selfMemberOption.Value);
}
}
}
}
void SetSpellTarget(TargetTypes targetType)
{
// Exclude target types based on effects added
if ((allowedTargets & targetType) == TargetTypes.None)
{
return;
}
// Clear buttons
casterOnlyButton.BackgroundTexture = null;
byTouchButton.BackgroundTexture = null;
singleTargetAtRangeButton.BackgroundTexture = null;
areaAroundCasterButton.BackgroundTexture = null;
areaAtRangeButton.BackgroundTexture = null;
// Set selected icon
switch (targetType)
{
case TargetTypes.CasterOnly:
casterOnlyButton.BackgroundTexture = casterOnlySelectedTexture;
break;
case TargetTypes.ByTouch:
byTouchButton.BackgroundTexture = byTouchSelectedTexture;
break;
case TargetTypes.SingleTargetAtRange:
singleTargetAtRangeButton.BackgroundTexture = singleTargetAtRangeSelectedTexture;
break;
case TargetTypes.AreaAroundCaster:
areaAroundCasterButton.BackgroundTexture = areaAroundCasterSelectedTexture;
break;
case TargetTypes.AreaAtRange:
areaAtRangeButton.BackgroundTexture = areaAtRangeSelectedTexture;
break;
}
selectedTarget = targetType;
UpdateSpellCosts();
}
/// <summary>
/// instance of <see cref="ParameterConfig"/>
/// </summary>
/// <param name="name"><see cref="Name"/></param>
/// <param name="tsource"><see cref="Tsource"/></param>
/// <param name="ttarget"><see cref="Ttarget"/></param>
/// <param name="offset"><see cref="Offset"/></param>
/// <param name="length"><see cref="Length"/></param>
/// <param name="scalar"><see cref="Scalar"/></param>
/// <param name="unit"><see cref="Unit"/></param>
/// <param name="label"><see cref="Label"/></param>
/// <param name="detail"><see cref="Detail"/></param>
/// <param name="isRevFlag"><see cref="IsRevFlag"/></param>
public ParameterConfig(
string name, SourceTypes tsource, TargetTypes ttarget,
byte offset, byte length, double scalar = 1,
string unit = "-", string label = "", string detail = "", bool isRevFlag = false)
{
Name = name;
Tsource = tsource;
Ttarget = ttarget;
Offset = offset;
Length = length;
Scalar = scalar;
Label = label;
Unit = unit;
Detail = detail;
IsRevFlag = isRevFlag;
}
public static bool CheckShipsTeam(GenericShip ship1, GenericShip ship2, TargetTypes targetTypes)
{
switch (targetTypes)
{
case TargetTypes.This:
return(IsSameShip(ship1, ship2));
case TargetTypes.OtherFriendly:
return(IsAnotherFriendly(ship1, ship2));
case TargetTypes.Enemy:
return(IsAnotherTeam(ship1, ship2));
case TargetTypes.Any:
return(true);
default:
return(true);
}
}
// public override Grid.Grid.Node[] FindTargets(Grid.Grid.Node wielder, Grid.Grid grid)
// {
// var neighbours = grid.GetNeighbours(wielder);
//
// // todo(chris) consider optimising as this will be used by every mellee enemy on the grid
// return neighbours.Where(node => node.Occupants.Any(it => it.Tags.Intersect(TargetTypes).Any())).ToArray();
// }
// todo(chris) shouldnt this only find targets in direction??
public override GridOccupant[] FindTargets(GridOccupant wielder, Vector2Int direction, Grid.Grid grid)
{
var node = new Grid.Grid.Node();
if (grid.TryGetNodeAt(wielder.Position, ref node))
{
var neighbours = grid.GetNeighbours(node);
var targets = new List <GridOccupant>();
foreach (var neighbour in neighbours)
{
var validOccupants = neighbour.Occupants.Where(it => TargetTypes.Contains(it.Type));
targets.AddRange(validOccupants);
}
return(targets.ToArray());
}
return(null);
}
public void RefreshState()
{
Owner = actor.Owner;
// PERF: Reuse collection to avoid allocations.
TargetTypes.Clear();
TargetTypes.UnionWith(actor.GetEnabledTargetTypes());
if (health != null)
{
HP = health.HP;
DamageState = health.DamageState;
}
var tooltip = tooltips.FirstEnabledTraitOrDefault();
if (tooltip != null)
{
TooltipInfo = tooltip.TooltipInfo;
TooltipOwner = tooltip.Owner;
}
}
/// <summary>
/// Return the endpoint to caller method
/// <param name="type">v3.0 Mailchimp EndPoint targetType, example: reports, lists etc</param>
/// <param name="subType" optional>Expects id for particular list/campaign etc</param>
/// <param name="id" optional>Expects id for particular list/campaign etc</param>
/// <param name="linkId" optional>NOT IMPLEMENTED YET</param>
/// </summary>
public static string EndPoint(TargetTypes type, SubTargetType subType, SubTargetType childSubType, string id = "", string param2 = "")
{
string targetType = EnumMapper.MapTarget(type);
string subCategory = EnumMapper.Map(subType);
string subChildCategory = EnumMapper.Map(childSubType);
var dataCenter = GetDatacenterPrefix();
if (id != "")
{
if (subCategory != "")
{
if (param2 != "")
{
if (subChildCategory != "")
{
return String.Format("https://{0}.api.mailchimp.com/3.0/{1}/{2}/{3}/{4}/{5}", dataCenter, targetType, id, subCategory, param2, childSubType);
}
else
{
return String.Format("https://{0}.api.mailchimp.com/3.0/{1}/{2}/{3}/{4}", dataCenter, targetType, id, subCategory, param2);
}
}
else
{
return String.Format("https://{0}.api.mailchimp.com/3.0/{1}/{2}/{3}", dataCenter, targetType, id, subCategory);
}
}
else
{
return String.Format("https://{0}.api.mailchimp.com/3.0/{1}/{2}", dataCenter, targetType, id);
}
}
else
{
return String.Format("https://{0}.api.mailchimp.com/3.0/{1}", dataCenter, targetType);
}
}
public void MakeSquares(TargetTypes thisTargetType, int rangeStart, int rangeEnd, int xCenter, int yCenter, bool MakeTargetSquares)
{
for(int i = rangeStart; i <= rangeEnd; i++) {
switch(thisTargetType) {
case TargetTypes.diamond:
for(int j = -i; j <= i; j++) {
for(int k = -i; k <= i; k++) {
if((Mathf.Abs(j) + Mathf.Abs(k)) == i) {
MoveTheSquareHere(j + xCenter, k + yCenter, MakeTargetSquares);
}
}
}
break;
case TargetTypes.cross:
for(int j = -i; j <= i; j++) {
for(int k = -i; k <= i; k++) {
if((Mathf.Abs (j) + Mathf.Abs (k)) == i && (k==0 | j==0)) {
MoveTheSquareHere(j + xCenter, k + yCenter, MakeTargetSquares);
}
}
}
break;
case TargetTypes.diagonal:
for(int j = -i; j <= i; j++) {
for(int k = -i; k <= i; k++) {
if((Mathf.Abs (j)==i && Mathf.Abs (k)==i)) {
MoveTheSquareHere(j + xCenter, k + yCenter, MakeTargetSquares);
}
}
}
break;
case TargetTypes.square:
for(int j = -i; j <= i; j++) {
for(int k = -i; k <= i; k++) {
if((Mathf.Abs (j)==i | Mathf.Abs (k)==i)) {
MoveTheSquareHere(j + xCenter, k + yCenter, MakeTargetSquares);
}
}
}
break;
}
}
}
public ConfirmOOIMessage(int targetId, TargetTypes targetType, int robotId)
{
this.targetID = targetId;
this.targetType = targetType;
robotID = robotId;
}
/// <summary>
/// Method uses displayTemplateClient to resolve which display template should be displayed
/// based on current context
/// </summary>
/// <param name="type">Target type (Item, Category)</param>
/// <param name="displayObject">The display object.</param>
/// <returns>display template name</returns>
private string GetDisplayTemplate(TargetTypes type, object displayObject)
{
// set the context variable
var set = UserHelper.CustomerSession.GetCustomerTagSet();
//Add Tags for category
switch (type)
{
case TargetTypes.Category:
var category = displayObject as CategoryBase;
if (category != null)
{
set.Add(AppConfigContext.CategoryId, new Tag(category.CategoryId));
}
break;
case TargetTypes.Item:
var item = displayObject as Item;
if (item != null)
{
set.Add(AppConfigContext.ItemId, new Tag(item.ItemId));
set.Add(AppConfigContext.ItemType, new Tag(item.GetType().Name));
}
break;
}
var viewName = _templateClient.GetDisplayTemplate(type, set);
return string.IsNullOrEmpty(viewName) ? type.ToString() : viewName;
}
private int FindMostAssociatedTarget(Vector2 xyPos, TargetTypes type, Matrix Xx2, Matrix Sx2, Matrix zSCR)
{
int sigma = 3;
int targetIdx = -2;
Dictionary<int, double> validIdxToDistance = new Dictionary<int, double>(); // index of targets that has this point within 3-sigma bound
//go through every target I have and do the sigma ellipse tseting
//if (type == TargetTypes.ConfirmedMOOI || type == TargetTypes.PotentialMOOI)
//{
//foreach (TargetTrackingImpl impl in implList)
//{
// //if (impl.Type == type || impl.Type == TargetTypes.ConfirmedMOOI || impl.Type == TargetTypes.Junk || impl.Type == TargetTypes.Meta)
// //{
// // check if the xyPos is within 3-sigma range
// Matrix zMinusMu = new Matrix(2, 1);
// zMinusMu[0, 0] = xyPos.X - impl.TargetState[0, 0];
// zMinusMu[1, 0] = xyPos.Y - impl.TargetState[1, 0];
// Matrix cov = impl.TargetCov.Submatrix(0, 1, 0, 1) * impl.TargetCov.Submatrix(0, 1, 0, 1).Transpose();
// if ((zMinusMu.Transpose() * cov.Inverse * zMinusMu)[0, 0] - (sigma * sigma) < 0)
// {
// double distance = Math.Sqrt(zMinusMu[0, 0] * zMinusMu[0, 0] + zMinusMu[1, 0] * zMinusMu[1, 0]);
// validIdxToDistance.Add(implList.IndexOf(impl), distance);
// targetIdx = implList.IndexOf(impl);
// }
// //}
//}
//}
/*
else if (type == TargetTypes.PotentialSOOI || type == TargetTypes.ConfirmedSOOI)
{
* */
//zSCRMean.Clear(); zSCRCov.Clear();
foreach (TargetTrackingImpl impl in implList)
{
//double distance = Double.MaxValue;
Matrix mean, cov;
//if (impl.Type == TargetTypes.PotentialSOOI || impl.Type == TargetTypes.ConfirmedSOOI || impl.Type == TargetTypes.Junk)
//{
impl.TestAssociation(Xx2, Sx2, out mean, out cov);
Vector2 twoDPose = new Vector2();
twoDPose.X = mean[0, 0]; twoDPose.Y = mean[1, 0];
Matrix zMinusMu = new Matrix(2, 1);
zMinusMu[0, 0] = xyPos.X - twoDPose.X;
zMinusMu[1, 0] = xyPos.Y - twoDPose.Y;
Matrix targetCov = impl.TargetCov.Submatrix(0, 1, 0, 1) * impl.TargetCov.Submatrix(0, 1, 0, 1).Transpose();
if ((zMinusMu.Transpose() * targetCov.Inverse * zMinusMu)[0, 0] - (sigma * sigma) < 0)
{
double distance = Math.Sqrt(zMinusMu[0, 0] * zMinusMu[0, 0] + zMinusMu[1, 0] * zMinusMu[1, 0]);
validIdxToDistance.Add(implList.IndexOf(impl), distance);
targetIdx = implList.IndexOf(impl);
}
zSCRMean.Add(twoDPose);
zSCRCov.Add(cov);
}
//}
//*/
// if found more than one valid targets, do the mean comparison
if (validIdxToDistance.Count > 1)
{
double min = Double.MaxValue;
foreach (KeyValuePair<int, double> pair in validIdxToDistance)
{
if (pair.Value < min)
{
min = pair.Value;
targetIdx = pair.Key;
}
}
}
else if (validIdxToDistance.Count == 0) // only one target to associate found
targetIdx = -1;
return targetIdx;
}
/// <summary>
/// Square Root, Sigma Points Information filter function. This estimates the information (of target) at next timestep (k+1) with given input at time k.
/// </summary>
/// <param name="xPOI">[7 x 1: east north up vel heading turnRate accleration]</param>
/// <param name="xx2">[9 x 1 : 3 NAV, 3ATT, 3GIM] = [E N U, roll pitch yaw, pan tilt scan]</param>
/// <param name="SPOI">[7 x 7 square root covariance of POI]</param>
/// <param name="Sw">[3 x 3 square root covariance of sensor noise: altitude, turnRate, and accleration]</param>
/// <param name="SMeasurement">[2 x 1 covariance vector of measurement]</param>
/// <param name="Sx2">[9 x 9 square root covariance of x2]</param>
/// <param name="zSCR">[2 x 1 Actual measurement of screen (pixels)]</param>
/// <param name="sigma_f">scaling factor for the distance of the sigmap points from the mean</param>
/// <returns></returns>
public void Update(Matrix xPOI, Matrix xx2, Matrix SPOI, Matrix Sw, Matrix SMeasurement, Matrix Sx2, Matrix zSCR, double sigma_f, string screenSize, string cameraType, TargetTypes type)
{
//////////////////////////////// INITIALIZATION /////////////////////////////////////
// SORRY - almost impossible to debug.
// variable names are mostly following IEEE Transaction paper written by Dr. Mark Campbell.
// also the variable names strictly follow the names I used in MATLAB code.
int nPOI = xPOI.Rows;
int nw = Sw.Rows;
int nLaser = 1;
// state matrix
Matrix xHat_a0 = new Matrix(nPOI + nw, 1);
for (int i = 0; i < nPOI; i++)
xHat_a0[i, 0] = xPOI[i, 0];
for (int i = 0; i < nw; i++)
xHat_a0[i + nPOI, 0] = 0;
// covariance matrix
Matrix S_a0 = new Matrix(nPOI + nw, nPOI + nw);
for (int i = 0; i < nPOI; i++)
S_a0[i, i] = SPOI[i, i];
for (int i = nPOI; i < nPOI + nw; i++)
S_a0[i, i] = Sw[i - nPOI, i - nPOI];
// generate 2*(nPOI + nw) + 1 sigma points
Matrix Xa0 = new Matrix(nPOI + nw, 2 * (nPOI + nw) + 1);
Matrix halfSigmaPoints1 = new Matrix(nPOI + nw, nPOI + nw);
Matrix halfSigmaPoints2 = new Matrix(nPOI + nw, nPOI + nw);
Matrix ones = new Matrix(1, nPOI + nw);
ones.Ones();
halfSigmaPoints1 = xHat_a0 * ones + S_a0 * sigma_f;
halfSigmaPoints2 = xHat_a0 * ones - S_a0 * sigma_f;
Xa0.SetSubMatrix(0, nPOI + nw - 1, 0, 0, xHat_a0);
Xa0.SetSubMatrix(0, nPOI + nw - 1, 1, (1 + nPOI + nw) - 1, halfSigmaPoints1);
Xa0.SetSubMatrix(0, nPOI + nw - 1, nPOI + nw + 1, 2 * (nPOI + nw), halfSigmaPoints2);
// dividing into two parts
Matrix XPOI_0 = Xa0.Submatrix(0, nPOI - 1, 0, Xa0.Columns - 1);
Matrix Xw_0 = Xa0.Submatrix(nPOI, Xa0.Rows - 1, 0, Xa0.Columns - 1);
// weight factors
double Wm0 = (sigma_f * sigma_f - (nPOI + nw)) / (sigma_f * sigma_f);
double Wc0 = Wm0 + 3 - sigma_f * sigma_f / (nPOI + nw);
double W = 1 / (2 * sigma_f * sigma_f);
//////////////////////////// SR-SPIF PREDICTION ////////////////////////////////
Matrix predPOIs = predPOI(XPOI_0, Xw_0, dT, type); // predicted sigma points
Matrix xhatPOIm = predPOIs.Submatrix(0, nPOI - 1, 0, 0) * Wm0;
for (int i = 1; i < predPOIs.Columns; i++)
{
xhatPOIm += predPOIs.Submatrix(0, nPOI - 1, i, i) * W;
}
// re-arrange sigma points
Matrix XPOI_Cm = new Matrix(nPOI, 2 * (nPOI + nw) + 1);
for (int i = 0; i < XPOI_Cm.Columns; i++)
{
for (int j = 0; j < XPOI_Cm.Rows; j++)
{
XPOI_Cm[j, i] = predPOIs[j, i] - xhatPOIm[j, 0];
}
}
/////////////////////// CONVERSION FROM PREDICTION TO UPDATE ///////////////////////
// define x_x2
Matrix x_nav = xx2.Submatrix(0, 2, 0, 0);
Matrix x_att = xx2.Submatrix(3, 5, 0, 0);
Matrix x_gim = xx2.Submatrix(6, 8, 0, 0);
Matrix S_nav = Sx2.Submatrix(0, 2, 0, 2);
Matrix S_att = Sx2.Submatrix(3, 5, 3, 5);
Matrix S_gim = Sx2.Submatrix(6, 8, 6, 8);
int nSCR = 2; int nx2 = xx2.Rows; int n2a = nPOI + nw + nSCR + nx2 + nLaser;
Matrix xhat_x2_m = new Matrix(nx2, 1); // creating x_x2 vector
for (int i = 0; i < 3; i++)
{
xhat_x2_m[i, 0] = x_nav[i, 0];
xhat_x2_m[i + 3, 0] = x_att[i, 0];
xhat_x2_m[i + 6, 0] = x_gim[i, 0];
}
Matrix S_x2_m = new Matrix(nx2, nx2); // creating augmented covariance diagonal matrix for x2
for (int i = 0; i < 3; i++)
{
S_x2_m[i, i] = S_nav[i, i];
S_x2_m[i + 3, i + 3] = S_att[i, i];
S_x2_m[i + 6, i + 6] = S_gim[i, i];
}
// equation 20
Matrix X_aug_c0m = new Matrix(nPOI + nSCR + nLaser + nx2, 1);
X_aug_c0m.Zero();
for (int i = 0; i < nPOI; i++)
X_aug_c0m[i, 0] = XPOI_Cm[i, 0];
Matrix X_aug_cm = new Matrix(nPOI + nSCR + nLaser + nx2, 2 * n2a); X_aug_cm.Zero();
X_aug_cm.SetSubMatrix(0, nPOI - 1, 0, XPOI_Cm.Columns - 2, XPOI_Cm.Submatrix(0, nPOI - 1, 1, XPOI_Cm.Columns - 1));
ones = new Matrix(1, 2 * nSCR + nLaser); ones.Ones();
X_aug_cm.SetSubMatrix(0, nPOI - 1, XPOI_Cm.Columns - 1, XPOI_Cm.Columns - 1 + (2 * nSCR + nLaser) - 1, XPOI_Cm.Submatrix(0, nPOI - 1, 0, 0) * ones);
ones = new Matrix(1, 2 * nx2); ones.Ones();
X_aug_cm.SetSubMatrix(0, nPOI - 1, XPOI_Cm.Columns - 1 + 2 * (nSCR + nLaser), XPOI_Cm.Columns - 1 + 2 * (nSCR + nLaser + nx2) - 1, XPOI_Cm.Submatrix(0, nPOI - 1, 0, 0) * ones);
X_aug_cm.SetSubMatrix(nPOI, nPOI + nSCR + nLaser - 1, XPOI_Cm.Columns - 1, XPOI_Cm.Columns - 1 + nSCR + nLaser - 1, SMeasurement);
X_aug_cm.SetSubMatrix(nPOI, nPOI + nSCR + nLaser - 1, XPOI_Cm.Columns - 1 + nSCR + nLaser, XPOI_Cm.Columns - 1 + 2 * (nSCR + nLaser) - 1, SMeasurement * -1);
X_aug_cm.SetSubMatrix(nPOI + nSCR + nLaser, nPOI + nSCR + nLaser + nx2 - 1, XPOI_Cm.Columns - 1 + 2 * (nSCR + nLaser), XPOI_Cm.Columns - 1 + 2 * (nSCR + nLaser) + nx2 - 1, Sx2);
X_aug_cm.SetSubMatrix(nPOI + nSCR + nLaser, nPOI + nSCR + nLaser + nx2 - 1, XPOI_Cm.Columns - 1 + 2 * (nSCR + nLaser) + nx2, XPOI_Cm.Columns - 1 + 2 * (nSCR + nLaser + nx2) - 1, Sx2 * -1);
// equation 21
Matrix X_aug_m = new Matrix(X_aug_c0m.Rows, X_aug_c0m.Columns + X_aug_cm.Columns);
Matrix meanVector = new Matrix(nPOI + nSCR + nLaser + nx2, 1);
meanVector.Zero();
meanVector.SetSubMatrix(0, nPOI - 1, 0, 0, xhatPOIm);
meanVector.SetSubMatrix(nPOI + nSCR + nLaser, nPOI + nSCR + nLaser + nx2 - 1, 0, 0, xx2);
ones = new Matrix(1, 2 * n2a); ones.Ones();
// create augmented sigma points
X_aug_m.SetSubMatrix(0, X_aug_m.Rows - 1, 0, 0, X_aug_c0m + meanVector);
X_aug_m.SetSubMatrix(0, X_aug_m.Rows - 1, 1, (2 * n2a) + 1 - 1, X_aug_cm + meanVector * ones);
// weight configurationU
double Wm0_aug = (sigma_f * sigma_f - n2a) / (sigma_f * sigma_f);
double Wc0_aug = Wm0_aug + 3 - (sigma_f * sigma_f / n2a);
////////////////////////////////////// SR-SPIF UPDATE /////////////////////////////////////////////
Matrix rangeNoise = new Matrix(1, 1); rangeNoise[0, 0] = SMeasurement[2, 2];
Matrix Z_SCR = MeasurementFct(X_aug_m.Submatrix(0, nPOI - 1, 0, X_aug_m.Columns - 1),
X_aug_m.Submatrix(nPOI + nSCR + nLaser, X_aug_m.Rows - 1, 0, X_aug_m.Columns - 1),
X_aug_m.Submatrix(nPOI, nPOI + nSCR + nLaser - 1, 0, X_aug_m.Columns - 1), rangeNoise, cameraType, screenSize);
Matrix z_mean = new Matrix(nSCR + nLaser, 1); // measurement mean
for (int i = 0; i < Z_SCR.Columns; i++)
{
if (i == 0)
z_mean += Z_SCR.Submatrix(0, Z_SCR.Rows - 1, 0, 0) * Wm0_aug;
else
z_mean += Z_SCR.Submatrix(0, Z_SCR.Rows - 1, i, i) * W;
}
ones = new Matrix(1, Z_SCR.Columns); ones.Ones();
Matrix Z_SCR_c = Z_SCR - z_mean * ones;
// equation 27
//Matrix P_POIx2SCR = new Matrix(nPOI + nx2, nSCR);
Matrix augPOIx2cm = new Matrix(nPOI + nx2, X_aug_cm.Columns);
augPOIx2cm.SetSubMatrix(0, nPOI - 1, 0, X_aug_cm.Columns - 1, X_aug_cm.Submatrix(0, nPOI - 1, 0, X_aug_cm.Columns - 1));
augPOIx2cm.SetSubMatrix(nPOI, nPOI + nx2 - 1, 0, X_aug_cm.Columns - 1, X_aug_cm.Submatrix(nPOI + nSCR + nLaser, nPOI + nSCR + nLaser + nx2 - 1, 0, X_aug_cm.Columns - 1));
Matrix augPOIx2c0m = new Matrix(nPOI + nx2, 1);
augPOIx2c0m.SetSubMatrix(0, nPOI - 1, 0, 0, X_aug_c0m.Submatrix(0, nPOI - 1, 0, 0));
augPOIx2c0m.SetSubMatrix(nPOI, nPOI + nx2 - 1, 0, 0, X_aug_c0m.Submatrix(nPOI + nSCR + nLaser, nPOI + nSCR + nLaser + nx2 - 1, 0, 0));
Matrix P_POIx2SCR = augPOIx2cm * Z_SCR_c.Submatrix(0, Z_SCR_c.Rows - 1, 1, Z_SCR_c.Columns - 1).Transpose() * W
+ augPOIx2c0m * Z_SCR_c.Submatrix(0, Z_SCR_c.Rows - 1, 0, 0).Transpose() * Wc0_aug;
//// Equation 28 and below chol part is equation 15
QrDecomposition qrDecomposition = new QrDecomposition(XPOI_Cm.Submatrix(0, XPOI_Cm.Rows - 1, 1, XPOI_Cm.Columns - 1).Transpose());
Matrix R_X_POI_Cm = qrDecomposition.UpperTriangularFactor;
CholeskyDecomposition cholUpdate;
Matrix S_POI_m;
if (Wc0_aug < 0)
{
cholUpdate = new CholeskyDecomposition(R_X_POI_Cm.Transpose() * R_X_POI_Cm * Math.Sqrt(W) * Math.Sqrt(W) - XPOI_Cm.Submatrix(0, nPOI - 1, 0, 0) * XPOI_Cm.Submatrix(0, nPOI - 1, 0, 0).Transpose() * Math.Abs(Wc0));
S_POI_m = cholUpdate.LeftTriangularFactor.Transpose();
}
else if (Wc0_aug > 0)
{
cholUpdate = new CholeskyDecomposition(R_X_POI_Cm.Transpose() * R_X_POI_Cm * Math.Sqrt(W) * Math.Sqrt(W) + XPOI_Cm.Submatrix(0, nPOI - 1, 0, 0) * XPOI_Cm.Submatrix(0, nPOI - 1, 0, 0).Transpose() * Math.Abs(Wc0));
S_POI_m = cholUpdate.LeftTriangularFactor.Transpose();
}
else
throw new Exception("Weight is zero");
//Matrix S_POI_mVer2 = new Matrix(nPOI, nPOI);
for (int i = 0; i < nPOI; i++)
{
//S_POI_mVer2[i, i] = Math.Abs(S_POI_m[i, i]);
S_POI_m[i, i] = Math.Abs(S_POI_m[i, i]);
}
QrDecomposition qrSPOImVer2 = new QrDecomposition(S_POI_m.Transpose().Inverse);
Matrix R_POIminus = qrSPOImVer2.UpperTriangularFactor;
Matrix Y_POIminus = R_POIminus.Transpose() * R_POIminus;
//QrDecomposition qrDecomposition_S_x2_m = new QrDecomposition(S_x2_m.Inverse.Transpose());
//Matrix R_x2m = qrDecomposition_S_x2_m.UpperTriangularFactor;
//QrDecomposition qrDecomposition_S_POI_m = new QrDecomposition(S_POI_m.Inverse.Transpose());
//Matrix R_POIm = qrDecomposition_S_POI_m.UpperTriangularFactor;
//Matrix P_POIm1 = R_POIm.Transpose() * R_POIm;
//Matrix P_x2m1 = R_x2m.Transpose() * R_x2m;
//Matrix blkDiag = new Matrix(P_POIm1.Rows + P_x2m1.Rows, P_POIm1.Columns + P_x2m1.Columns);
//blkDiag.SetSubMatrix(0, P_POIm1.Rows - 1, 0, P_POIm1.Columns - 1, P_POIm1);
//blkDiag.SetSubMatrix(P_POIm1.Rows, P_POIm1.Rows + P_x2m1.Rows - 1, P_POIm1.Columns, P_POIm1.Columns + P_x2m1.Columns - 1, P_x2m1);
//Matrix C_SCR = P_POIx2SCR.Transpose() * blkDiag;
// Equation 28
QrDecomposition qrInvSx2 = new QrDecomposition(Sx2.Inverse);
Matrix R_x2minus = qrInvSx2.UpperTriangularFactor;
Matrix Y_x2minus = R_x2minus.Transpose() * R_x2minus;
Matrix blkDiagYseries = new Matrix(nPOI + nx2, nPOI + nx2);
blkDiagYseries.SetSubMatrix(0, nPOI - 1, 0, nPOI - 1, Y_POIminus);
blkDiagYseries.SetSubMatrix(nPOI, nPOI + nx2 - 1, nPOI, nPOI + nx2 - 1, Y_x2minus);
Matrix C = P_POIx2SCR.Transpose() * blkDiagYseries;
// SUPER IMPORTANT STEP !!
QrDecomposition anotherQrDecomposition = new QrDecomposition(Z_SCR_c.Submatrix(0, Z_SCR_c.Rows - 1, 1, Z_SCR_c.Columns - 1).Transpose());
Matrix Rvtilde = anotherQrDecomposition.UpperTriangularFactor;
CholeskyDecomposition anotherCholUpdate;
Matrix Svtilde;
if (Wc0_aug < 0)
{
anotherCholUpdate = new CholeskyDecomposition(Rvtilde.Transpose() * Rvtilde * Math.Sqrt(W) * Math.Sqrt(W) - Z_SCR_c.Submatrix(0, Z_SCR_c.Rows - 1, 0, 0) * Z_SCR_c.Submatrix(0, Z_SCR_c.Rows - 1, 0, 0).Transpose() * Math.Abs(Wc0_aug));
Svtilde = anotherCholUpdate.LeftTriangularFactor.Transpose();
}
else if (Wc0_aug > 0)
{
anotherCholUpdate = new CholeskyDecomposition(Rvtilde.Transpose() * Rvtilde * Math.Sqrt(W) * Math.Sqrt(W) + Z_SCR_c.Submatrix(0, Z_SCR_c.Rows - 1, 0, 0) * Z_SCR_c.Submatrix(0, Z_SCR_c.Rows - 1, 0, 0).Transpose() * Math.Abs(Wc0_aug));
Svtilde = anotherCholUpdate.LeftTriangularFactor.Transpose();
}
else
throw new Exception("Weight is zero");
//Matrix SvtildeVer2 = new Matrix(Svtilde.Rows, Svtilde.Columns); SvtildeVer2.Zero();
for (int i = 0; i < Svtilde.Columns; i++)
Svtilde[i, i] = Math.Abs(Svtilde[i, i]);
Matrix P_SCR = Svtilde.Transpose() * Svtilde;
Matrix Y_SCR = P_SCR.Inverse;
CholeskyDecomposition lastCholDecompose = new CholeskyDecomposition(Y_SCR);
Matrix R_SCR = lastCholDecompose.LeftTriangularFactor.Transpose();
// Equation 31-33
//QrDecomposition qrDecomposition_SSCR = new QrDecomposition(SSCR.Transpose().Inverse);
//Matrix R_SCR = qrDecomposition_SSCR.UpperTriangularFactor;
Matrix v_kp1 = zSCR - z_mean;
Matrix i_kp1 = new Matrix(nx2 + nSCR + nLaser, 1);
Matrix xhatPOIm_xhat_x2_m_aug = new Matrix(nPOI + nx2, 1);
xhatPOIm_xhat_x2_m_aug.SetSubMatrix(0, nPOI - 1, 0, 0, xhatPOIm);
xhatPOIm_xhat_x2_m_aug.SetSubMatrix(nPOI, nPOI + nx2 - 1, 0, 0, xhat_x2_m);
i_kp1.SetSubMatrix(0, nx2 - 1, 0, 0, R_x2minus * xhat_x2_m);
i_kp1.SetSubMatrix(nx2, nx2 + nSCR + nLaser - 1, 0, 0, R_SCR * (v_kp1 + C * xhatPOIm_xhat_x2_m_aug));
Matrix I_kp1 = R_SCR * C;
// equation 34
// actually [blkDiag; I_kp1];
Matrix blkDiag;
blkDiag = new Matrix(R_POIminus.Rows + R_x2minus.Rows + I_kp1.Rows, R_POIminus.Columns + R_x2minus.Columns);
blkDiag.SetSubMatrix(0, R_POIminus.Rows - 1, 0, R_POIminus.Columns - 1, R_POIminus);
blkDiag.SetSubMatrix(R_POIminus.Rows, R_POIminus.Rows + R_x2minus.Rows - 1, R_POIminus.Columns, R_POIminus.Columns + R_x2minus.Columns - 1, R_x2minus);
blkDiag.SetSubMatrix(R_POIminus.Rows + R_x2minus.Rows, blkDiag.Rows - 1, 0, blkDiag.Columns - 1, I_kp1);
QrDecomposition qrDecomposition_blkDiag = new QrDecomposition(blkDiag);
Matrix R_blob = qrDecomposition_blkDiag.UpperTriangularFactor;
Matrix T_kp1 = qrDecomposition_blkDiag.OrthogonalFactor;
Matrix RPOI = R_blob.Submatrix(0, nPOI - 1, 0, nPOI - 1);
Matrix RPOIx2 = R_blob.Submatrix(0, nPOI - 1, nPOI, R_blob.Columns - 1);
Matrix Rx2 = R_blob.Submatrix(nPOI, R_blob.Rows - 1, nPOI, R_blob.Columns - 1);
// final calculation
Matrix ninvRPOI_RPOIx2_invRx2 = (RPOI.Inverse * -1) * RPOIx2 * Rx2.Inverse;
Matrix R_POIm_xhatPOIm_i_kp1 = new Matrix(nPOI + i_kp1.Rows, 1);
R_POIm_xhatPOIm_i_kp1.SetSubMatrix(0, nPOI - 1, 0, 0, R_POIminus * xhatPOIm);
R_POIm_xhatPOIm_i_kp1.SetSubMatrix(nPOI, R_POIm_xhatPOIm_i_kp1.Rows - 1, 0, 0, i_kp1);
Matrix invRPOI_ninvRPOI_RPOIx2_invRx2 = new Matrix(RPOI.Rows, RPOI.Columns + ninvRPOI_RPOIx2_invRx2.Columns);
invRPOI_ninvRPOI_RPOIx2_invRx2.SetSubMatrix(0, nPOI - 1, 0, nPOI - 1, RPOI.Inverse);
invRPOI_ninvRPOI_RPOIx2_invRx2.SetSubMatrix(0, nPOI - 1, nPOI, invRPOI_ninvRPOI_RPOIx2_invRx2.Columns - 1, ninvRPOI_RPOIx2_invRx2);
Matrix xhatPOI = invRPOI_ninvRPOI_RPOIx2_invRx2 * T_kp1.Transpose() * R_POIm_xhatPOIm_i_kp1;
Matrix invRPOI_ninvRPOI_RPOIx2_invRx2_zeros_invRx2 = new Matrix(invRPOI_ninvRPOI_RPOIx2_invRx2.Rows + nx2 + Rx2.Rows, invRPOI_ninvRPOI_RPOIx2_invRx2.Columns);
invRPOI_ninvRPOI_RPOIx2_invRx2_zeros_invRx2.SetSubMatrix(0, invRPOI_ninvRPOI_RPOIx2_invRx2.Rows - 1, 0, invRPOI_ninvRPOI_RPOIx2_invRx2.Columns - 1, invRPOI_ninvRPOI_RPOIx2_invRx2);
invRPOI_ninvRPOI_RPOIx2_invRx2_zeros_invRx2.SetSubMatrix(invRPOI_ninvRPOI_RPOIx2_invRx2.Rows, invRPOI_ninvRPOI_RPOIx2_invRx2.Rows + nx2 - 1, nPOI, invRPOI_ninvRPOI_RPOIx2_invRx2.Columns - 1, Rx2.Inverse);
QrDecomposition qrDecomposition_invRPOIstuff = new QrDecomposition(invRPOI_ninvRPOI_RPOIx2_invRx2_zeros_invRx2);
Matrix S_blob = qrDecomposition_invRPOIstuff.UpperTriangularFactor;
Matrix newSPOI = S_blob.Submatrix(0, nPOI - 1, 0, nPOI - 1);
targetState = xhatPOI;
if (Math.Abs(newSPOI[0, 0]) < Math.Sqrt(0.05))
newSPOI[0, 0] = Math.Sqrt(0.05) * Math.Sign(newSPOI[0, 0]);
if (Math.Abs(newSPOI[1, 1]) < Math.Sqrt(0.06))
newSPOI[1, 1] = Math.Sqrt(0.05) * Math.Sign(newSPOI[1, 1]);
S_target = newSPOI;
}
public DynamicTarget()
{
type = TargetTypes.NOTARGET;
}
public static void MouseTargeting(TargetTypes targetingType, int cursorID)
{
setTargeting(targetingType);
}
public static double FindTargetDistance(ILidarScan<ILidar2DPoint> lidarScan, double u, double v, Dictionary<int, int> colorPix, Vector2 TLCorner, Vector2 RBCorner,
string cameraSize, string camType, RobotPose camPose, TargetTypes type, out ILidar2DPoint lidarPt, ref List<Vector2> ptInBox)
{
#region camera stuff
Matrix DCM4D = new Matrix(4, 4, 1.0);
double[] fc = new double[2];
double[] cc = new double[2];
if (cameraSize.Equals("320x240"))
{
//for 320 x 240 image with Unibrain Fire-i camera
if (camType.Equals("Fire-i"))
{
fc[0] = 384.4507; fc[1] = 384.1266;
cc[0] = 155.1999; cc[1] = 101.5641;
}
// Fire-Fly MV
else if (camType.Equals("FireFly"))
{
fc[0] = 345.26498; fc[1] = 344.99438;
cc[0] = 159.36854; cc[1] = 118.26944;
}
}
else if (cameraSize.Equals("640x480"))
{
// for 640 x 480 image with Unibrain Fire-i camera
if (camType.Equals("Fire-i"))
{
fc[0] = 763.5805; fc[1] = 763.8337;
cc[0] = 303.0963; cc[1] = 215.9287;
}
// for Fire-Fly MV (Point Gray)
else if (camType.Equals("FireFly"))
{
fc[0] = 691.09778; fc[1] = 690.70187;
cc[0] = 324.07388; cc[1] = 234.22204;
}
}
double alpha_c = 0;
// camera matrix
Matrix KK = new Matrix(fc[0], alpha_c * fc[0], cc[0], 0, fc[1], cc[1], 0, 0, 1);
#endregion
// update DCM for point transformation
DCM4D[0, 3] = camPose.x; DCM4D[1, 3] = camPose.y; DCM4D[2, 3] = camPose.z;
DCM4D[0, 0] = Math.Cos(camPose.yaw); DCM4D[1, 1] = Math.Cos(camPose.yaw);
DCM4D[0, 1] = Math.Sin(camPose.yaw); DCM4D[1, 0] = -Math.Sin(camPose.yaw);
List<Vector2> pixelList = new List<Vector2>(lidarScan.Points.Count);
List<ILidar2DPoint> lidarScanInBox = new List<ILidar2DPoint>();
foreach (ILidar2DPoint pt in lidarScan.Points)
{
Matrix point = new Matrix(4, 1);
point[0, 0] = -pt.RThetaPoint.ToVector4().Y;
point[1, 0] = pt.RThetaPoint.ToVector4().X;
point[2, 0] = pt.RThetaPoint.ToVector4().Z;
point[3, 0] = 1;
Matrix transPt = DCM4D * point;
Matrix ptImgPlane = new Matrix(3, 1);
ptImgPlane[0, 0] = transPt[0, 0] / transPt[1, 0];
ptImgPlane[1, 0] = -transPt[2, 0] / transPt[1, 0];
ptImgPlane[2, 0] = transPt[1, 0] / transPt[1, 0];
ptImgPlane = KK * ptImgPlane;
pixelList.Add(new Vector2(ptImgPlane[0, 0], ptImgPlane[1, 0]));
if (ptImgPlane[0, 0] >= TLCorner.X && ptImgPlane[0, 0] <= RBCorner.X && ptImgPlane[1, 0] >= TLCorner.Y && ptImgPlane[1, 0] <= RBCorner.Y)
{
if (colorPix.Count > 0)
{
if (colorPix.ContainsKey((int)ptImgPlane[0, 0]) && colorPix[(int)ptImgPlane[0, 0]] == 255)
{
lidarScanInBox.Add(pt);
ptInBox.Add(new Vector2(ptImgPlane[0, 0], ptImgPlane[1, 0]));
}
}
else
{
lidarScanInBox.Add(pt);
ptInBox.Add(new Vector2(ptImgPlane[0, 0], ptImgPlane[1, 0]));
}
}
}
if (lidarScanInBox.Count == 0)
{
lidarPt = null;
return -1;
}
lidarPt = FineTargetDistanceClusterBased(lidarScanInBox);
if (lidarPt == null)
return -1;
return lidarPt.RThetaPoint.R;
}
/// <summary>
/// Update tracker with given input. It will do simple data association with given data (comparing distance between existing targets).
/// </summary>
/// <param name="uPixel">x-pixel in a camera coordinate</param>
/// <param name="vPixel">y-pixel in a camera coordinate</param>
/// <param name="range">laser range reading</param>
/// <param name="currentPose"></param>
/// <returns>target index = targetID</returns>
public int Update(double uPixel, double vPixel, RobotPose currentPose, SensorPose lidarPose, TargetTypes type, ILidar2DPoint lidarPt, string cameraType, int numCamera)
{
Matrix zSCR, Xx2;
double distance = 0;
int targetIdx = 0;
lock (locker)
{
// Compute X and Y position of the target based on the passed lidar point
//Vector2 xyPos = TargetTrackingImpl.FindPosCoord(screenSize, range, uPixel, vPixel, currentPose, null);
// this X Y coordinate = E N
double yaw = currentPose.yaw - Math.PI / 2;
double pitch = currentPose.pitch; double roll = currentPose.roll;
Matrix R_ENU2R = new Matrix(Math.Cos(yaw), Math.Sin(yaw), 0, -Math.Sin(yaw), Math.Cos(yaw), 0, 0, 0, 1) *
new Matrix(1, 0, 0, 0, Math.Cos(pitch), -Math.Sin(pitch), 0, Math.Sin(pitch), Math.Cos(pitch)) *
new Matrix(Math.Cos(roll), 0, Math.Sin(roll), 0, 1, 0, -Math.Sin(roll), 0, Math.Cos(roll));
Matrix localPt = new Matrix(3, 1); localPt[0, 0] = -lidarPt.RThetaPoint.ToVector2().Y - lidarPose.y; localPt[1, 0] = lidarPt.RThetaPoint.ToVector2().X + lidarPose.x;
Matrix globalPt = R_ENU2R.Inverse * localPt;
Vector2 xyPos = new Vector2(globalPt[0, 0] + currentPose.x, globalPt[1, 0] + currentPose.y);
this.xyPosList.Add(xyPos);
#region debugging - go away
// find the closest target
//if (currentTargetPose.Count == 0)
//{
// distance = double.MaxValue;
// targetIdx = 0;
//}
//else
//{
// targetIdx = FindClosestTarget(xyPos, ref distance, type);
//}
//if (type == TargetTypes.PotentialSOOI || type == TargetTypes.ConfirmedSOOI)
//{
// if (distance > staticDistanceThreshold) // if the distance is larger than the threshold, add as a new target
// {
// if (currentTargetPose.Count == currentTargetPose.Capacity)
// currentTargetPose.RemoveAt(0);
// currentTargetPose.Add(xyPos);
// implList.Add(new TargetTrackingImpl(sigma_f, dT, screenSize, cameraType));
// targetIdx = currentTargetPose.Count - 1;
// }
//}
//else if (type == TargetTypes.PotentialMOOI || type == TargetTypes.ConfirmedMOOI)
//{
// if (distance > dynamicDistanceThreshold) // if the distance is larger than the threshold, add as a new target
// {
// if (currentTargetPose.Count == currentTargetPose.Capacity)
// currentTargetPose.RemoveAt(0);
// currentTargetPose.Add(xyPos);
// implList.Add(new TargetTrackingImpl(sigma_f, dT, screenSize, cameraType));
// targetIdx = currentTargetPose.Count - 1;
// }
//}
#endregion
zSCR = new Matrix(3, 1);
zSCR[0, 0] = uPixel; zSCR[1, 0] = vPixel; zSCR[2, 0] = lidarPt.RThetaPoint.R;
Xx2 = new Matrix(9, 1);
Xx2[0, 0] = currentPose.x; Xx2[1, 0] = currentPose.y; /* Xx2[2, 0] = currentPose.z; */ Xx2[2, 0] = 0;
Xx2[3, 0] = currentPose.roll; Xx2[4, 0] = currentPose.pitch; Xx2[5, 0] = currentPose.yaw;
//Xx2[3, 0] = 0; Xx2[4, 0] = 0; Xx2[5, 0] = currentPose.yaw;
Xx2[6, 0] = 0; Xx2[7, 0] = 0; Xx2[8, 0] = 0;
// modification for 3 cameras
if (numCamera == 3)
{
if (uPixel > 0 && uPixel <= 320)
{
Xx2[6, 0] = 50 * Math.PI / 180;
}
else if (uPixel > 320 && uPixel <= 320 * 2)
{
zSCR[0, 0] = uPixel - 320; zSCR[1, 0] = vPixel;
Xx2[6, 0] = 1.5 * Math.PI / 180;
}
else if (uPixel > 320 * 2)
{
zSCR[0, 0] = uPixel - 320 * 2; zSCR[1, 0] = vPixel;
Xx2[6, 0] = -47 * Math.PI / 180;
}
}
// find the most associated target
Matrix Sxx2 = new Matrix(9, 9);
Sxx2[0, 0] = Math.Sqrt(Math.Abs(currentPose.covariance[0, 0]));
Sxx2[1, 1] = Math.Sqrt(Math.Abs(currentPose.covariance[1, 1]));
Sxx2[2, 2] = Math.Sqrt(Math.Abs(currentPose.covariance[2, 2]));
Sxx2[3, 3] = Math.Sqrt(0.01); // roll
Sxx2[4, 4] = Math.Sqrt(0.01); // pitch
Sxx2[5, 5] = Math.Sqrt(0.03); // yaw
Sxx2[6, 6] = Math.Sqrt(0.01); // pan
Sxx2[7, 7] = Math.Sqrt(0.01); // tilt
Sxx2[8, 8] = Math.Sqrt(0.01); // scan
//targetIdx = FindMostAssociatedTarget(xyPos, type, Xx2, Sxx2, zSCR);
Matrix zSCRForTest = new Matrix(2, 1); zSCRForTest[0, 0] = lidarPt.RThetaPoint.R; zSCRForTest[1, 0] = lidarPt.RThetaPoint.theta;
targetIdx = FindMostAssociatedTarget(xyPos, type, Xx2, Sxx2, zSCRForTest);
if (type == TargetTypes.PotentialSOOI || type == TargetTypes.ConfirmedSOOI)
{
if (targetIdx == -1) // if no good associated target found, then add a new one
{
implList.Add(new TargetTrackingImpl(sigma_f, dT, screenSize, cameraType));
targetIdx = implList.Count - 1;
}
}
else if (type == TargetTypes.PotentialMOOI || type == TargetTypes.ConfirmedMOOI)
{
if (targetIdx == -1)
{
implList.Add(new TargetTrackingImpl(sigma_f, dT, screenSize, cameraType));
targetIdx = implList.Count - 1;
}
}
Matrix xhatPOI = new Matrix(7, 1); xhatPOI.Zero();
xhatPOI[0, 0] = xyPos.X; xhatPOI[1, 0] = xyPos.Y; xhatPOI[2, 0] = 0;
if (!implList[targetIdx].IsInitialized)
implList[targetIdx].SetInitialPOIInfo(xhatPOI);
}
Matrix Sx2 = new Matrix(9, 9);
Sx2[0, 0] = Math.Sqrt(Math.Abs(currentPose.covariance[0, 0]));
Sx2[1, 1] = Math.Sqrt(Math.Abs(currentPose.covariance[1, 1]));
Sx2[2, 2] = Math.Sqrt(Math.Abs(currentPose.covariance[2, 2]));
//Sx2[0, 0] = Math.Sqrt(0.1);
//Sx2[1, 1] = Math.Sqrt(0.1);
//Sx2[2, 2] = Math.Sqrt(0.01);
Sx2[3, 3] = Math.Sqrt(0.01); // roll
Sx2[4, 4] = Math.Sqrt(0.01); // pitch
Sx2[5, 5] = Math.Sqrt(0.01); // yaw
Sx2[6, 6] = Math.Sqrt(0.01); // pan
Sx2[7, 7] = Math.Sqrt(0.01); // tilt
Sx2[8, 8] = Math.Sqrt(0.01); // scan
// update with correct target
implList[targetIdx].UpdateZSCR(zSCR);
implList[targetIdx].UpdateVehicleState(Xx2, currentPose.timestamp);
implList[targetIdx].UpdateSx2(Sx2);
if (implList[targetIdx].Type != TargetTypes.ConfirmedSOOI && implList[targetIdx].Type != TargetTypes.ConfirmedMOOI)
implList[targetIdx].SetTargetType(type);
implList[targetIdx].Update();
return targetIdx;
}
public int UpdateTypeWithConfirmation(int targetIdx, TargetTypes type)
{
if (implList.Count <= 0 || implList[targetIdx] == null)
return 1;
implList[targetIdx].SetTargetType(type);
return 0;
}
/// <summary>
/// Return the closest point in a list from this point
/// </summary>
/// <param name="list">list of points</param>
/// <param name="distance">distance to the closest point</param>
/// <returns>Closest point of the list</returns>
private Vector2 ClosestInListByKind(Vector2 pt, List<Vector2> list, ref double distance, TargetTypes type)
{
double minDist = Double.MaxValue;
Vector2 toReturn = new Vector2();
foreach (Vector2 v in list)
{
if (implList[list.IndexOf(v)].Type == type && v.DistanceTo(pt) < minDist)
{
minDist = v.DistanceTo(pt);
toReturn = v;
}
}
distance = minDist;
return toReturn;
}
public void MakeSquares(TargetTypes thisTargetType, int rangeStart, int rangeEnd, bool MakeTargetSquares)
{
MakeSquares (thisTargetType, rangeStart, rangeEnd, player.xPosition, player.yPosition, MakeTargetSquares);
}
public void SetTargets(TargetTypes targets)
{
ShowText("Select your target");
switch (targets)
{
case TargetTypes.Enemy:
_boxEnemy.SetAllTarget();
break;
case TargetTypes.Self:
_boxChar.SetSelfTarget();
break;
case TargetTypes.All:
_boxChar.SetSelfTarget();
_boxEnemy.SetAllTarget();
break;
case TargetTypes.AllEnemies:
_boxEnemy.SetAllTarget();
break;
default:
_boxChar.CancelAllTargets();
_boxEnemy.CancelAllTargets();
break;
}
}
static void setTargeting(TargetTypes targetingType)
{
_TargettingType = targetingType;
// Set the UI's cursor to a targetting cursor. If multi, tell the cursor which multi.
_ui.Cursor.IsTargeting = true;
// Stop continuous movement.
_ContinuousMoveCheck = false;
}
/// <summary>
/// Predict next sigma points using dynamic model
/// </summary>
/// <param name="XPOI">Sigma points of POI: [nPOI x (2 * (nPOI+nw) + 1)]</param>
/// <param name="Xw">Sigma points of noise: [nw x (2 * (nPOI+nw) + 1)]</param>
/// <param name="dT">time interval for integration (discrete system)</param>
/// <param name="type">Indicating what kind of target it is</param>
/// <returns></returns>
private Matrix predPOI(Matrix XPOI, Matrix Xw, double dT, TargetTypes type)
{
// state: [x y z velocity heading turn-rate accleration]
Matrix toReturn = new Matrix(XPOI.Rows, XPOI.Columns);
for (int i = 0; i < toReturn.Columns; i++)
{
for (int j = 0; j < toReturn.Rows; j++)
{
if (type == TargetTypes.PotentialSOOI || type == TargetTypes.ConfirmedSOOI || type == TargetTypes.Junk || type == TargetTypes.Meta)
{
if (j == 5)
toReturn[j, i] = Xw[1, i];
else if (j == 6)
toReturn[j, i] = Xw[2, i];
else
toReturn[j, i] = XPOI[j, i];
}
else if (type == TargetTypes.PotentialMOOI || type == TargetTypes.ConfirmedMOOI)
{
if (j == 5)
toReturn[j, i] = Xw[1, i];
else if (j == 6)
toReturn[j, i] = Xw[2, i];
else
toReturn[j, i] = XPOI[j, i] + normDist.NextDouble() * dT; // random walk????
}
}
}
return toReturn;
}
static void clearTargeting()
{
// Clear our target cursor.
_TargettingType = TargetTypes.Nothing;
_ui.Cursor.IsTargeting = false;
}
public void SetTargetType(TargetTypes type)
{
this.type = type;
}
public static string MapTarget(TargetTypes type)
{
return type.ToString().Replace("_", "-");
}
public IRCMessage(string p_message)
{
rawMessage = p_message;
if (rawMessage.StartsWith("PING "))
{
messageType = "PING";
messageString = rawMessage.Split(' ')[1].TrimStart(':');
}
else
{
messageList = rawMessage.Split(' ').ToList<string>();
messageType = messageList[1];
if (messageList[0].Contains('!'))
{
string[] userArray = messageList[0].Split('!');
User.Name = userArray[0].TrimStart(':');
string[] userHostArray = userArray[1].Split('@');
User.User = userHostArray[0];
User.Hostmask = userHostArray[1];
}
else
{
User.Name = messageList[0].TrimStart(':');
User.User = "";
User.Hostmask = "";
}
if (messageType == "JOIN")
{
TargetType = TargetTypes.CHANNEL;
replyTo = messageList[2].TrimStart(':');
}
else if (messageType == "PRIVMSG" || messageType == "TOPIC")
{
if (messageList[2].TrimStart(':').StartsWith("#"))
{
TargetType = TargetTypes.CHANNEL;
replyTo = messageList[2].TrimStart(':').ToLowerInvariant();
}
else
{
TargetType = TargetTypes.USER;
replyTo = User.Name.ToLowerInvariant();
}
messageString = String.Join(" ", messageList.ToArray(), 3, messageList.Count - 3);
if (messageString.StartsWith(":"))
{
messageString = messageString.Substring(1, messageString.Length - 1);
}
if (MessageString.StartsWith(Convert.ToChar((byte)1).ToString()))
{
int messageEnd = MessageString.IndexOf(Convert.ToChar((byte)1), 1);
if (messageEnd > 1)
{
ctcpString = MessageString.Substring(1, messageEnd - 1);
ctcp = true;
}
}
}
}
}
//public void GetTargetInfo(out List<RobotPose> state, out List<RobotPose> lastPose, out List<Matrix> cov)
//{
// List<RobotPose> trackedState = new List<RobotPose>(currentTargetPose.Count);
// List<RobotPose> lastRobotPose = new List<RobotPose>(currentTargetPose.Count);
// List<Matrix> trackedCov = new List<Matrix>(currentTargetPose.Count);
// foreach (TargetTrackingImpl impl in implList)
// {
// trackedState.Add(new RobotPose(impl.TargetState[0, 0], impl.TargetState[1, 0], impl.TargetState[2, 0], 0, 0, 0, currentPose.timestamp));
// lastRobotPose.Add(impl.LastRobotPose);
// trackedCov.Add(impl.TargetCov);
// }
// state = trackedState;
// lastPose = lastRobotPose;
// cov = trackedCov;
// //messaging.SendTargets(robotID, trackedState, trackedCov, lastRobotPose);
//}
private int FindClosestTarget(Vector2 xyPos, ref double distance, TargetTypes type)
{
//Vector2 closestTarget = xyPos.ClosestInList(currentTargetPose, ref distance);
Vector2 closestTarget = ClosestInListByKind(xyPos, currentTargetPose, ref distance, type);
return currentTargetPose.IndexOf(closestTarget);
}
