public Rover(int x, int y)
{
orientation = new OrientationNorth();
X = x;
Y = y;
coordinate = new Coordinate(x, y);
}
/// <summary>
/// Sets state
/// </summary>
/// <param name="baseFrame">Base frame</param>
/// <param name="relative">Relative frame</param>
public override void Set(ReferenceFrame baseFrame, ReferenceFrame relative)
{
base.Set(baseFrame, relative);
IOrientation bo = baseFrame as IOrientation;
IOrientation ro = relative as IOrientation;
IVelocity bv = baseFrame as IVelocity;
IVelocity rv = relative as IVelocity;
IAngularVelocity oa = baseFrame as IAngularVelocity;
IAngularVelocity ra = relative as IAngularVelocity;
double[] vb = bv.Velocity;
double[] vr = rv.Velocity;
double[,] mb = bo.Matrix;
double[,] m = ro.Matrix;
double[] om = oa.Omega;
double[] pos = relative.Position;
Vector3D.StaticExtensionVector3D.VectorPoduct(om, pos, hv);
for (int i = 0; i < 3; i++)
{
velocity[i] = vb[i];
for (int j = 0; j < 3; j++)
{
velocity[i] += mb[i, j] * (vr[j] + hv[j]);
}
}
}
public void GetDiffrenece_Should_Return_Same_With_Expected_Vector2(IOrientation orientation, Vector2 expectedDifference)
{
//arrange
//act
//assert
Assert.Equal(expectedDifference, orientation.GetDiffrenece());
}
public bool TryParse(string unparsedDirection, out IOrientation orientation)
{
switch (unparsedDirection)
{
case ("NORTH"):
orientation = new Orientation(CompassDirection.North);
return(true);
case ("SOUTH"):
orientation = new Orientation(CompassDirection.South);
return(true);
case ("EAST"):
orientation = new Orientation(CompassDirection.East);
return(true);
case ("WEST"):
orientation = new Orientation(CompassDirection.West);
return(true);
default:
orientation = null;
return(false);
}
}
public void Right_Should_Update_Rovers_Orientation_To_Right_Neigbour(IOrientation orientation, Orientations expectedOrientation)
{
//arrange
//act
//assert
Assert.Equal(expectedOrientation, orientation.Right().GetOrientationType());
}
public void GetOrientation_Should_Return_Same_With_Expected_Orientation(IOrientation orientation, Orientations expectedOrientation)
{
//arrange
//act
//assert
Assert.Equal(expectedOrientation, orientation.GetOrientationType());
}
void IDifferentialEquationSolver.CopyVariablesToSolver(int offset, double[] variables)
{
int n = offset;
for (int i = 0; i < aggrWrappres.Length; i++)
{
AggregableWrapper aw = aggrWrappres[i];
IAggregableMechanicalObject ao = aw.Aggregate;
double[] state = ao.State;
for (int j = 0; j < state.Length; j++)
{
double a = variables[n];
state[j] = a;
if (j < measures.Length)
{
AggergateDerivation der = measures[j] as AggergateDerivation;
der.Set(j, a);
}
++n;
}
Motion6DAcceleratedFrame frame = aw.OwnFrame;
IOrientation or = frame;
Array.Copy(state, 6, or.Quaternion, 0, 4);
IAngularVelocity w = frame;
Array.Copy(state, 10, w.Omega, 0, 3);
}
}
public static Rover CreateRover(string definition)
{
if (string.IsNullOrEmpty(definition)) throw new WrongRoverDefinitionException("Definition of rover is empty.");
var coordinatesAndOrientation = definition.Split(' ');
if(coordinatesAndOrientation.Length != 3) throw new WrongRoverDefinitionException("Definition of rover doesn't have the proper format.");
Coordinates coordinates;
IOrientation orientation;
try
{
coordinates = new Coordinates(Convert.ToInt32(coordinatesAndOrientation[0]),
Convert.ToInt32(coordinatesAndOrientation[1]));
orientation = OrientationFactory.GenerateOrientation(coordinatesAndOrientation[2]);
}
catch (Exception ex)
{
throw new WrongRoverDefinitionException("Bad definition.", ex);
}
var rover = new Rover();
rover.Init(coordinates, orientation);
return rover;
}
public OrientationManager
(
IOrientation startingOrientation
, IDictionary <string, IOrientation> orientations
)
{
_currentOrientation = startingOrientation;
_orientations = orientations;
}
public Rover(IOrientation orientation, int maxX, int maxY, int initialX, int initialY)
{
this.orientation = orientation;
this.maxX = maxX;
this.maxY = maxY;
this.X = initialX;
this.Y = initialY;
}
public ICommandPerformer CreatePlacePerformer(
int xCoordinate,
int yCoordinate,
IOrientation orientation)
{
return(new PlacePerformer(
this.robotStateFactory,
this.tableDimensions,
xCoordinate,
yCoordinate,
orientation));
}
public PlacePerformer(
IRobotStateFactory robotStateFactory,
ITableDimensions tableDimensions,
int xCoordinate,
int yCoordinate,
IOrientation orientation)
{
this.robotStateFactory = robotStateFactory;
this.tableDimensions = tableDimensions;
this.xCoordinate = xCoordinate;
this.yCoordinate = yCoordinate;
this.orientation = orientation;
}
/// <summary>
/// Sets aliases
/// </summary>
private void SetAliases()
{
ReferenceFrame own = Own;
IVelocity v = own as IVelocity;
IOrientation o = own as IOrientation;
IAngularVelocity av = own as IAngularVelocity;
aln.SetAliases(0, own.Position, 0, 3);
aln.SetAliases(3, v.Velocity, 0, 3);
aln.SetAliases(6, o.Quaternion, 0, 4);
aln.SetAliases(10, av.Omega, 0, 3);
aln.SetAliases(13, relativeVelocity, 0, 3);
aln.SetAliases(16, omega, 0, 3);
}
/* private object sd()
* {
* return result[9, 1];
* }
*
* private object md()
* {
* return result[10, 1];
* }
*
* private object nd()
* {
* return result[11, 1];
* }
*
* private object od()
* {
* return result[12, 1];
* }
*
* private object ld()
* {
* return result[13, 1];
* }*/
#endregion
private void SetRelative()
{
ReferenceFrame relative = Relative;
Array.Copy(relativePosition, relative.Position, 3);
IVelocity v = relative as IVelocity;
// Array.Copy(relativeVelocity, v.RevativeVelocity, 3);
Array.Copy(relativeQuaternion, relative.Quaternion, 3);
IOrientation or = relative as IOrientation;
Array.Copy(relativeQuaternion, or.Quaternion, 4);
relative.SetMatrix();
}
public Robot(int positionX, int positionY, IOrientation orientation)
{
if (positionX < 0)
{
throw new ArgumentOutOfRangeException(nameof(positionX), "Must be greater than or equal to zero");
}
if (positionY < 0)
{
throw new ArgumentOutOfRangeException(nameof(positionY), "Must be greater than or equal to zero");
}
PositionX = positionX;
PositionY = positionY;
Orientation = orientation;
}
/// <summary>
/// Sets state
/// </summary>
/// <param name="baseFrame">Base frame</param>
/// <param name="relative">Relative frame</param>
public override void Set(ReferenceFrame baseFrame, ReferenceFrame relative)
{
base.Set(baseFrame, relative);
IAngularVelocity ab = baseFrame as IAngularVelocity;
IAngularVelocity ar = relative as IAngularVelocity;
IOrientation ore = relative as IOrientation;
double[,] m = ore.Matrix;
double[] ob = ab.Omega;
double[] or = ar.Omega;
for (int i = 0; i < omega.Length; i++)
{
omega[i] = or[i];
for (int j = 0; j < 3; j++)
{
omega[i] += m[i, j] * ob[j];
}
}
}
/// <summary>
/// Sets state
/// </summary>
/// <param name="baseFrame">Base frame</param>
/// <param name="relative">Relative frame</param>
public override void Set(ReferenceFrame baseFrame, ReferenceFrame relative)
{
base.Set(baseFrame, relative);
IAcceleration ab = baseFrame as IAcceleration;
IAcceleration ar = relative as IAcceleration;
IAngularAcceleration arn = relative as IAngularAcceleration;
IVelocity vb = baseFrame as IVelocity;
IVelocity vr = relative as IVelocity;
IAngularVelocity anb = baseFrame as IAngularVelocity;
IAngularVelocity anr = relative as IAngularVelocity;
double[] rp = Position;
double[,] m = Matrix;
double[] omr = anr.Omega;
StaticExtensionVector3D.VectorPoduct(omr, vr.Velocity, tempV);
double om2 = StaticExtensionVector3D.Square(omr);
double[] eps = arn.AngularAcceleration;
StaticExtensionVector3D.VectorPoduct(eps, rp, temp);
for (int i = 0; i < 3; i++)
{
tempV[i] *= 2;
tempV[i] += om2 * rp[i] + relativeAcceleration[i] + temp[i];
}
RealMatrix.Multiply(m, tempV, acceleration);
double[] omb = anb.Omega;
IOrientation orr = relative as IOrientation;
double[,] mrr = orr.Matrix;
RealMatrix.Multiply(omb, mrr, temp);
StaticExtensionVector3D.VectorPoduct(temp, omr, tempV);
for (int i = 0; i < 3; i++)
{
temp[i] = eps[i] + tempV[i];
}
RealMatrix.Multiply(temp, m, angularAcceleration);
}
void GetParameters(IPosition p, ref IVelocity velocity, ref IOrientation orientation, ref IAngularVelocity om)
{
velocity = null;
orientation = null;
om = null;
IPosition pa = p;
if (p is IReferenceFrame)
{
IReferenceFrame f = p as IReferenceFrame;
pa = f.Own;
}
if (pa is IVelocity)
{
velocity = pa as IVelocity;
}
else
{
velocity = null;
}
if (pa is IOrientation)
{
orientation = pa as IOrientation;
}
else
{
orientation = null;
}
if (pa is IAngularVelocity)
{
om = pa as IAngularVelocity;
}
else
{
om = null;
}
}
public void TurnRight()
{
orientation = Orientation.TurnRight();
}
public void UpdateOrientation(IOrientation orientation)
{
_orientation = orientation ?? throw new ArgumentNullException();
}
public IRobotStateBuilder WithCompassDirection(CompassDirection newCompassDirection)
{
this.orientation = new Orientation(newCompassDirection);
return(this);
}
public RobotStateBuilder(int xCoordinate, int yCoordinate, IOrientation orientation)
{
this.xCoordinate = xCoordinate;
this.yCoordinate = yCoordinate;
this.orientation = orientation;
}
public IRobotState Create(int xCoordinate, int yCoordinate, IOrientation orientation)
{
return(new RobotState(xCoordinate, yCoordinate, orientation));
}
public Rover(IOrientation orientation, int maxX, int maxY)
: this(orientation, maxX, maxY, 0, 0)
{
}
public void TurnLeft()
{
_currentOrientation = _orientations[_currentOrientation.Name].Left;
}
public void TurnRight()
{
_currentOrientation = _orientations[_currentOrientation.Name].Right;
}
public void TurnLeft()
{
orientation = Orientation.TurnLeft();
}
void IDifferentialEquationSolver.CalculateDerivations()
{
Normalize(aggregate);
foreach (AggregableWrapper aw in aggrWrappres)
{
IAggregableMechanicalObject obj = aw.Aggregate;
if (obj is Diagram.UI.IUpdatableObject)
{
Diagram.UI.IUpdatableObject uo = obj as Diagram.UI.IUpdatableObject;
if (uo.Update != null)
{
uo.Update();
}
}
}
Solve();
int n = 0;
int kv = 0;
for (int i = 0; i < aggrWrappres.Length; i++)
{
AggregableWrapper wrapper = aggrWrappres[i];
IAggregableMechanicalObject agg = wrapper.Aggregate;
Motion6DAcceleratedFrame frame = wrapper.OwnFrame;
IVelocity vel = frame;
IAcceleration acc = frame;
IPosition pos = frame;
double[] state = agg.State;
double[] p = pos.Position;
double[] v = vel.Velocity;
for (int j = 0; j < 3; j++)
{
p[j] = state[j];
derivations[n + j, 0] = state[j];
double a = state[j + 3];
v[j] = a;
derivations[n + j, 1] = a;
derivations[n + 3 + j, 0] = a;
derivations[n + 3 + j, 1] = vector[kv];
++kv;
}
IOrientation or = frame;
double[] q = or.Quaternion;
for (int j = 0; j < 4; j++)
{
double a = state[j + 6];
quater[j] = a;
q[j] = a;
}
IAngularVelocity av = frame;
double[] om = av.Omega;
for (int j = 0; j < 3; j++)
{
double a = state[j + 10];
omega[j] = a;
om[j] = a;
}
StaticExtensionVector3D.CalculateQuaternionDerivation(quater, omega, der, auxQuaternion);
for (int j = 0; j < 4; j++)
{
derivations[n + 6 + j, 0] = quater[j];
derivations[n + 6 + j, 1] = der[j];
}
for (int j = 0; j < 3; j++)
{
derivations[n + 10 + j, 0] = omega[j];
derivations[n + 10 + j, 1] = vector[kv];
++kv;
}
int kk = n + 13;
int stk = kk;
int stv = 6;
int sk = 13;
for (int j = 13; j < agg.Dimension; j++)
{
derivations[kk, 0] = state[sk];
double a = state[sk + 1];
derivations[kk, 1] = a;
++kk;
++stk;
++sk;
derivations[kk, 0] = a;
derivations[kk, 1] = vector[kv];
++sk;
++kv;
++stv;
++kk;
++j;
}
n += agg.Dimension;
}
}
public Rover()
{
orientation = new NoOrientation();
}
public void SetOrientation(IOrientation o)
{
orientation = o;
}
public void Init(Coordinates coordinates, IOrientation orientation)
{
this.coordinates = coordinates;
this.orientation = orientation;
}
/// <summary>
/// Updates itself
/// </summary>
public override void Update()
{
try
{
this.FullReset();
IDataConsumer cons = this;
cons.UpdateChildrenData();
ReferenceFrame relative = Relative;
IPosition p = relative;
double[] x = p.Position;
ReferenceFrame parent = this.GetParentFrame();
for (int i = 0; i < 3; i++)
{
x[i] = (double)measurements[i].Parameter();
}
if (relative is IVelocity)
{
IVelocity vel = relative as IVelocity;
double[] v = vel.Velocity;
for (int i = 0; i < 3; i++)
{
IDerivation d = measurements[i] as IDerivation;
v[i] = Measurement.GetDouble(d.Derivation);
}
}
double[] qua = relative.Quaternion;
for (int i = 0; i < qua.Length; i++)
{
IMeasurement m = measurements[i + 3];
qua[i] = (double)m.Parameter();
}
relative.SetMatrix();
if (relative is IAngularVelocity)
{
IAngularVelocity av = relative as IAngularVelocity;
double[] om = av.Omega;
IOrientation or = relative as IOrientation;
for (int i = 0; i < 4; i++)
{
IDerivation d = measurements[i + 3] as IDerivation;
der[i] = Measurement.GetDouble(d.Derivation);
}
Vector3D.StaticExtensionVector3D.CalculateDynamics(or.Quaternion, der, om, qd);
}
if (relative is IAcceleration)
{
IAcceleration acc = relative as IAcceleration;
IAngularAcceleration anc = relative as IAngularAcceleration;
double[] linacc = acc.RelativeAcceleration;
for (int i = 0; i < linacc.Length; i++)
{
linacc[i] = Measurement.GetDouble(secondDeriM[i]);
}
for (int i = 0; i < 4; i++)
{
angsec[i] = Measurement.GetDouble(secondDeriM[i + 3]);
}
IAngularVelocity av = relative as IAngularVelocity;
double[] om = av.Omega;
IOrientation or = relative as IOrientation;
double[] angacc = anc.AngularAcceleration;
Vector3D.StaticExtensionVector3D.CalculateAcceleratedDynamics(or.Quaternion, der, om, qd, angsec, angacc);
}
base.Update();
}
catch (Exception exception)
{
exception.ShowError(10);
//!!! OLD this.Throw(ex);
}
}
