private void CorrectModelPosition(Module2DModel nmModel, ModuleBehavior nmBehavior, Thing best, List <Thing> near)
{
//locations are not very accurate and accumulate errors
//whenever we encounter a landmark, we update the orientation/location of the model to correct errors
Segment s2 = Module2DModel.SegmentFromUKSThing(near[0]);
Segment s1 = null;
foreach (Link l in best.References)
{
Segment s = Module2DModel.SegmentFromUKSThing(l.T);
if (s.theColor == s2.theColor)
{
s1 = s; break;
}
}
if (s1 != null)
{
PointPlus m1 = s1.MidPoint;
PointPlus m2 = s2.MidPoint;
float deltaX = m1.X - m2.X;
float deltaY = m1.Y - m2.Y;
PointPlus a1 = new PointPlus()
{
P = (Point)(s1.P1.V - s1.P2.V)
};
PointPlus a2 = new PointPlus()
{
P = (Point)(s2.P1.V - s2.P2.V)
};
Angle deltaTheta = a1.Theta - a2.Theta;
nmModel.Move(-deltaX, -deltaY);
nmBehavior.TurnTo(deltaTheta);
}
}
public override void Fire()
{
Init(); //be sure to leave this here to enable use of the na variable
float[] dist = { .5f, .1f, 0, -.025f, -.1f };
float motionX = 0;
float motionY = 0;
if (na.Width < 3)
{
Initialize();
}
for (int i = 0; i < na.Height; i++)
{
if (na.GetNeuronAt(1, i).LastCharge > 0.9)
{
motionX = dist[i];
}
}
if (na.GetNeuronAt(1, 2).CurrentCharge != 0)
{
motionX = na.GetNeuronAt(1, 2).CurrentCharge;
na.GetNeuronAt(1, 2).SetValue(0);
}
if (na.GetNeuronAt(0, 2).LastCharge > 0.9)
{
motionY = 0.5f;
}
if (na.GetNeuronAt(2, 2).LastCharge > 0.9)
{
motionY = -0.5f;
}
//obsolete
ModuleRealityModel mrm = (ModuleRealityModel)FindModuleByType(typeof(ModuleRealityModel));
if (mrm != null)
{
mrm.Move(motionX);
}
Module3DSim m3D = (Module3DSim)FindModuleByType(typeof(Module3DSim));
if (m3D != null && motionX != 0)
{
m3D.Move(motionX);
}
bool moved = false;
Module2DSim m2D = (Module2DSim)FindModuleByType(typeof(Module2DSim));
if (m2D != null && motionX != 0 || motionY != 0)
{
moved = m2D.Move(motionX, motionY);
}
Module2DModel m2DModel = (Module2DModel)FindModuleByType(typeof(Module2DModel));
if (m2DModel != null && moved && motionX != 0 || motionY != 0)
{
m2DModel.Move(motionX, motionY);
}
Module2DVision m2DVision = (Module2DVision)FindModuleByType(typeof(Module2DVision));
if (m2DVision != null && motionX != 0)
{
m2DVision.ViewChanged();
}
}