private object Parse(string v, Type parameterType, string methodName, int index)
{
if (parameterType == typeof(string))
{
return(v);
}
if (parameterType == typeof(Frame3D))
{
return(Frame3D.Parse(v));
}
if (parameterType.IsEnum)
{
return(Enum.Parse(parameterType, v));
}
if (parameterType == typeof(int))
{
return(int.Parse(v));
}
if (parameterType == typeof(double))
{
return(double.Parse(v));
}
if (parameterType == typeof(Single))
{
return(Single.Parse(v));
}
if (parameterType == typeof(bool))
{
return(bool.Parse(v));
}
throw new Exception("Parameter type " + parameterType.Name + " is not supported. Method " + methodName + ", index " + index);
}
public Frame3D NextLocation()
{
Frame3D frame = _locations[_counter];
_counter++;
return(frame);
}
private void UpdateRelativeSpeeds()
{
foreach (var requestedId in requestedRelativeSpeed.Keys)
{
if (!ContainBody(requestedId))
{
continue;
}
var actorLocation = GetAbsoluteLocation(requestedId);
var requestedSpeed = requestedRelativeSpeed[requestedId];
// FIXME: rewrite transformations for 3d space
var speed = new Frame3D(
requestedSpeed.X * Math.Cos(actorLocation.Yaw.Radian),
requestedSpeed.X * Math.Sin(actorLocation.Yaw.Radian),
0,
requestedSpeed.Pitch,
requestedSpeed.Yaw,
requestedSpeed.Roll
).ToUnityBasis();
UpdateSpeed(requestedId, speed);
}
}
public ImageSensorData Measure()
{
var tmpLocation = GetCameraLocation(_robot);
var result = new KinectData(_settings.VerticalResolution, _settings.HorisontalResolution);
var horisontalAngle = -_settings.HorisontalViewAngle / 2.0;
var verticalAngle = -_settings.VerticalViewAngle / 2.0;
for (int i = 0; i < _settings.VerticalResolution; i++)
{
Frame3D mediateDirection = SensorRotation.VerticalFrameRotation(tmpLocation, -verticalAngle);
horisontalAngle = -_settings.HorisontalViewAngle / 2.0;
for (int j = 0; j < _settings.HorisontalResolution; j++)
{
Frame3D direction = SensorRotation.HorisontalFrameRotation(mediateDirection, horisontalAngle);
Ray ray = new Ray(tmpLocation.ToPoint3D(), SensorRotation.GetFrontDirection(direction));
var dist = double.PositiveInfinity;
foreach (var body in _worldRoot.GetSubtreeChildrenFirst())
{
if (_settings.Exclude.All(a => a != body))
{
var inter = Intersector.Intersect(body, ray);
dist = Math.Min(dist, inter);
}
}
result.Depth[i, j] = dist;
//verticalAngle += _settings.VStep;
horisontalAngle += _settings.HStep;
}
verticalAngle += _settings.VStep;
}
return(new ImageSensorData(result.GetBytes()));
}
public override TriggerKeep Act(double time)
{
if (time > endTime)
{
return(TriggerKeep.Remove);
}
if (robot.IsDisabled)
{
return(TriggerKeep.Remove);
}
var engine = robot.World.GetEngine <ICommonEngine>();
var robotAngle = engine.GetAbsoluteLocation(robot.ObjectId).Yaw.Radian;
var speed = new Frame3D(
linear * Math.Cos(robotAngle),
linear * Math.Sin(robotAngle),
0,
Angle.Zero,
Angle.FromGrad(angular),
Angle.Zero);
engine.SetAbsoluteSpeed(robot.ObjectId, speed);
ScheduledTime += interval;
return(TriggerKeep.Keep);
}
public static Frame3D ToCvarcBasis(this Frame3D unityCoordinates)
{
var u = unityCoordinates;
return(new Frame3D(u.Z / Centimeter, -u.X / Centimeter, u.Y / Centimeter,
-u.Pitch, -u.Yaw, -u.Roll));
}
public TopViewCamera(Frame3D loc, double aspectRatio) : base(aspectRatio)
{
Vector3 vec = loc.ToDirectXVector();
_topViewTransform = Matrix.LookAtLH(new Vector3(0, 0, vec.Z),
new Vector3(), Vector3.UnitY);
}
public MapItem(string type, Frame3D Location)
{
switch (type)
{
case "DR": Tag = "RedDetail"; break;
case "DB": Tag = "BlueDetail"; break;
case "DG": Tag = "GreenDetail"; break;
case "VW": Tag = "VerticalWall"; break;
case "VWR": Tag = "VerticalRedSocket"; break;
case "VWB": Tag = "VerticalBlueSocket"; break;
case "VWG": Tag = "VerticalGreenSocket"; break;
case "HW": Tag = "HorizontalWall"; break;
case "HWR": Tag = "HorizontalRedSocket"; break;
case "HWB": Tag = "HorizontalBlueSocket"; break;
case "HWG": Tag = "HorizontalGreenSocket"; break;
}
X = Location.X;
Y = Location.Y;
}
public void Attach(string objectToAttach, string host, Frame3D relativePosition)
{
this.Log("Attach", objectToAttach, host, relativePosition);
var parent = GameObject.Find(host);
var attachment = GameObject.Find(objectToAttach);
attachment.transform.position = parent.transform.position;
attachment.transform.rotation = parent.transform.rotation;
var rp = relativePosition.ToUnityBasis();
attachment.transform.position += Quaternion.Euler(parent.transform.eulerAngles) *
new Vector3((float)rp.X, (float)rp.Y, (float)rp.Z);
attachment.transform.rotation *= Quaternion.Euler((float)rp.Pitch.Grad, (float)rp.Yaw.Grad, (float)rp.Roll.Grad);
var joint = attachment.AddComponent <FixedJoint>();
joint.connectedBody = parent.GetComponent <Rigidbody>();
joint.enableCollision = false;
joint.breakForce = Single.PositiveInfinity;
joint.enablePreprocessing = false;
attachedParams.Add(attachment, new Tuple <float, float>(attachment.GetComponent <Rigidbody>().drag, attachment.GetComponent <Rigidbody>().angularDrag));
attachment.GetComponent <Rigidbody>().drag = attachment.GetComponent <Rigidbody>().angularDrag = 0;
}
/// <summary>
/// Поставит углы, схватит gimbal lock у pitch, но пока так, хоть Yaw верный.
/// </summary>
private static Frame3D SetAngles(Frame3D frame, BEPUphysics.MathExtensions.Matrix matrix)
{
AIRLab.Mathematics.Matrix m = new AIRLab.Mathematics.Matrix(4, 4);
m[0, 0] = matrix.M11;
m[0, 1] = matrix.M12;
m[0, 2] = matrix.M13;
m[0, 3] = 0.0f;
m[1, 0] = matrix.M21;
m[1, 1] = matrix.M22;
m[1, 2] = matrix.M23;
m[1, 3] = 0.0f;
m[2, 0] = matrix.M31;
m[2, 1] = matrix.M32;
m[2, 2] = matrix.M33;
m[2, 3] = 0.0f;
m[3, 0] = frame.X;
m[3, 1] = frame.Y;
m[3, 2] = frame.Z;
m[3, 3] = 1.0f;
var yaw = Geometry.Atan2(m[1, 0], m[0, 0]);
var pitch = Geometry.Atan2(-m[2, 0], Geometry.Hypot(m[2, 1], m[2, 2]));
var roll = Geometry.Atan2(m[2, 1], m[2, 2]);
Frame3D fr = new Frame3D(frame.X, frame.Y, frame.Z, pitch, yaw, roll);
return(fr);
}
private Frame3D CalculateSpeed(Frame3D startingLocation)
{
var speedX = Math.Cos(startingLocation.Yaw.Radian) * Rules.HookVelocity;
var speedY = Math.Sin(startingLocation.Yaw.Radian) * Rules.HookVelocity;
return(new Frame3D(speedX, speedY, 0));
}
public static Point3D GetFrontDirection(Frame3D frame)
{
Matrix m = frame.GetMatrix();
var front = new Point3D(m[0, 0], m[1, 0], m[2, 0]);
return(front);
}
public void VerticalTest(Frame3D frame, Angle angle, Point3D rotated)
{
Point3D computed = SensorRotation.VerticalRotation(frame, angle);
Assert.AreEqual(rotated, computed);
Console.WriteLine("Expected: {0}. But was: {1}", rotated, computed);
}
/// <summary>
/// Присоединит переданный объект, используя переданный Location.
/// </summary>
public void Attach(IPhysical body, Frame3D realLocation, bool joinWithFriction = true)
{
var bb = (BepuBody)body;
var rb = bb.RealBody;
BepuConverter.PutInFrame(rb, realLocation, bb.InitialOrientation);
//bb.RealBody.Position = new Vector3(-bb.RealBody.Position.X, bb.RealBody.Position.Y, bb.RealBody.Position.Z);
rb.Position += RealBody.Position;
rb.Orientation *= RealBody.Orientation;
SolverGroup wj = BepuWorld.MakeWeldJoint(RealBody, rb);
Connection c;
try //есть connection
{
c = _connections.First(x => x.OtherBody == rb);
c.Add(wj);
}
catch (Exception) //нет connection
{
c = new Connection(rb);
c.Add(wj);
_connections.Add(c); //connection добавляем для обоих тел! М.К.
bb._connections.Add(c);
}
}
void ApplyCommand(double time)
{
if (currentMovement == null)
{
return;
}
var engine = actor.World.GetEngine <ICommonEngine>();
var location = engine.GetAbsoluteLocation(actor.ObjectId);
var c = currentMovement;
var linear = Math.Sign(c.LinearVelocity) * Math.Min(Math.Abs(c.LinearVelocity), rules.LinearVelocityLimit);
var angular = Math.Sign(c.AngularVelocity.Radian) * Math.Min(Math.Abs(c.AngularVelocity.Radian), rules.AngularVelocityLimit.Radian);
var angle = location.Yaw.Radian;
if (linear != 0)
{
angular = 0;
}
var requestedSpeed = new Frame3D(
linear * Math.Cos(angle),
linear * Math.Sin(angle),
0,
Angle.Zero,
Angle.FromRad(angular),
Angle.Zero);
engine.SetAbsoluteSpeed(actor.ObjectId, requestedSpeed);
}
public void TestLocation()
{
var root = new Body();
var box = new Box();
root.Add(box);
var lo = new LoggingObject(box, root);
Movement movement1 = GetMovement(0, new Frame3D(1, 0, 0));
lo.Movements.Add(movement1);
Movement movement2 = GetMovement(300, new Frame3D(0, 1, 0));
lo.Movements.Add(movement2);
var ol = new ObjectLoader(lo, new Body());
Frame3D expectedLoc = Frame3D.Identity;
for (int i = 0; i <= _expectedLocations.Keys.Max(); i++)
{
if (_expectedLocations.ContainsKey(i))
{
expectedLoc = _expectedLocations[i];
}
ol.LoadLocation(i);
Assert.AreEqual(expectedLoc, ol._loadedBody.Location);
}
}
/// <summary>
/// Вернёт frame, в котором повёрнуты координаты X и Y и Yaw равен переданному углу.
/// </summary>
static public Frame3D Rotate2D(Frame3D frame, Angle angle)
{
double x = frame.X * Math.Cos(angle.Radian) - frame.Y * Math.Sin(angle.Radian);
double y = frame.X * Math.Sin(angle.Radian) + frame.Y * Math.Cos(angle.Radian);
return(new Frame3D(x, y, frame.Z, Angle.FromGrad(0), angle, Angle.FromGrad(0)));
}
public static Frame3D ToUnityBasis(this Frame3D cvarcCoordinates)
{
var c = cvarcCoordinates;
return(new Frame3D(-c.Y * Centimeter, c.Z * Centimeter, c.X * Centimeter,
-c.Pitch, -c.Yaw, -c.Roll));
}
public SlardarCommand GetCommand(Frame3D currentLocation)
{
var currentTarget = Type == "LeftTop" ? Trajectory[CurrentPointIndex] : Trajectory[CurrentPointIndex] * -1;
var distance = Geometry.Distance(currentLocation.ToPoint3D(), currentTarget.ToPoint3D());
if (distance < Epsilon)
{
CurrentPointIndex++;
CurrentPointIndex %= Trajectory.Count;
currentTarget = Type == "LeftTop" ? Trajectory[CurrentPointIndex] : Trajectory[CurrentPointIndex] * -1;
}
if (Trajectory[CurrentPointIndex].Z > 0)
{
var waitDuration = Trajectory[CurrentPointIndex].Z;
CurrentPointIndex++;
CurrentPointIndex %= Trajectory.Count;
return(new SlardarCommand {
GameMovement = new GameMovement {
WaitTime = waitDuration
}
});
}
var xAxisDeviation = currentLocation.Yaw.Simplify360().Grad;
var deltaX = currentTarget.X - currentLocation.X;
var deltaY = currentTarget.Y - currentLocation.Y;
var angleToTarget = Angle.FromGrad(Math.Atan2(deltaY, deltaX) * 180 / Math.PI).Simplify360().Grad;
var diff = Math.Abs(angleToTarget - xAxisDeviation);
var absDiff = diff > 360 - diff ? 360 - diff : diff;
var direction = Math.Abs(Angle.FromGrad(xAxisDeviation + absDiff).Simplify360().Grad - angleToTarget) < .1 ||
Math.Abs(Angle.FromGrad(xAxisDeviation + absDiff).Simplify360().Grad - angleToTarget - 360) < .1
? 1
: -1;
var isNotToBeRotated = Math.Abs((int)(absDiff / PudgeRules.Current.RotationAngle)) == 0;
if (isNotToBeRotated)
{
return new SlardarCommand
{
GameMovement = new GameMovement {
Range = Math.Min(15, distance)
}
}
}
;
return(new SlardarCommand
{
GameMovement = new GameMovement {
Angle = direction * Math.Min(absDiff, 15.0)
}
});
}
}
/// <summary>
/// Засунет объект в фрейм, переведя в координаты физики, повернув объект на нужные углы.
/// </summary>
public static void PutInFrame(Entity entity, Frame3D frame, Quaternion inititalOrientation)
{
entity.Position = ToSimUnits(Frame3DToVector3(frame.NewX(-frame.X)));
entity.Orientation = Quaternion.CreateFromYawPitchRoll(
//(float)frame.Pitch.Radian, (float)frame.Roll.Radian, -(float)frame.Yaw.Radian);
(float)frame.Roll.Radian, (float)frame.Pitch.Radian, -(float)frame.Yaw.Radian) * inititalOrientation;
}
/// <summary>
/// Создает камеру с видом от первого лица
/// </summary>
/// <param name="source">Тело, к которому привязана камера</param>
/// <param name="offset">Смещение камеры относительно Location тела</param>
/// <param name="viewAngle"> </param>
/// <param name="aspectRatio"> </param>
public FirstPersonCamera(Body source, Frame3D offset,
Angle viewAngle, double aspectRatio) : base(aspectRatio)
{
_source = source;
_offset = offset;
ViewAngle = viewAngle;
_defaultWorldMatrix = Matrix.LookAtLH(Vector3.Zero, new Vector3(-1, 0, 0), UpVector);
}
public Hook(IHookRobot pudge, IHookRules rules)
{
Pudge = Compatibility.Check <PudgeRobot>(this, pudge);
Speed = new Frame3D();
StartingLocation = new Frame3D();
Id = Pudge.World.IdGenerator.CreateNewId(this);
Rules = rules;
}
private static Matrix GetMatrix(Frame3D loc)
{
var m = new Matrix();
m.Translate((float)loc.X, -(float)loc.Y);
m.Rotate(-(float)loc.Yaw.Grad);
return(m);
}
/// <summary>
/// повернуть фрейм вокруг вертикальной оси (z)
/// </summary>
/// <param name="frame">положение сонара</param>
/// <param name="angle">угол поворота</param>
/// <returns>вектор - новое направление</returns>
public static Frame3D HorisontalFrameRotation(Frame3D frame, Angle angle)
{
Matrix m = frame.GetMatrix();
//var up = new Point3D(m[0, 2], m[1, 2], m[2, 2]);
var up = new Point3D(0, 0, 1);
Frame3D rotated = frame.Apply(Frame3D.DoRotate(up, angle));
return(rotated);
}
/// <summary>
/// повернуть фрейм вокруг горизонтальной оси (y)
/// </summary>
/// <param name="frame">положение сонара</param>
/// <param name="angle">угол поворота</param>
/// <returns>вектор - новое направление</returns>
public static Frame3D VerticalFrameRotation(Frame3D frame, Angle angle)
{
Matrix m = frame.GetMatrix();
//var right = new Point3D(m[0, 1], m[1, 1], m[2, 1]);
var right = new Point3D(0, -1, 0);
Frame3D rotated = frame.Apply(Frame3D.DoRotate(right, angle));
return(rotated);
}
private double Distance(Frame3D first, Frame3D second)
{
var x = Math.Abs(first.X - second.X);
var y = Math.Abs(first.Y - second.Y);
return(Math.Sqrt(x * x + y * y));
//var delta = first.Invert().Apply(second);
//return Math.Sqrt(delta.X * delta.X + delta.Y * delta.Y + delta.Z * delta.Z);
}
public static Point3D VerticalRotation(Frame3D frame, Angle angle)
{
Matrix m = frame.GetMatrix();
var right = new Point3D(m[0, 1], m[1, 1], m[2, 1]);
Frame3D rotated = frame.Apply(Frame3D.DoRotate(right, angle));
Matrix n = rotated.GetMatrix();
var rotatedFront = new Point3D(n[0, 0], n[1, 0], n[2, 0]);
return(rotatedFront);
}
public static Point3D HorisontalRotation(Frame3D frame, Angle angle)
{
Matrix m = frame.GetMatrix();
var up = new Point3D(m[0, 2], m[1, 2], m[2, 2]);
Frame3D rotated = frame.Apply(Frame3D.DoRotate(up, angle));
Matrix n = rotated.GetMatrix();
var rotatedFront = new Point3D(n[0, 0], n[1, 0], n[2, 0]);
return(rotatedFront);
}
/// <summary>
/// Отсоединит <paramref name="newChild"/> от его текущего родителя
/// и присоединит к данному телу, сохраняя при этом абсолютные координаты
/// </summary>
/// <param name="newChild"></param>
public void DetachAttachMaintaingLoction(Body newChild)
{
Frame3D childAbsolute = newChild.GetAbsoluteLocation();
if (newChild.Parent != null)
{
newChild.Parent.Remove(newChild);
}
newChild.Location = GetAbsoluteLocation().Invert().Apply(childAbsolute);
Add(newChild);
}
static Func <IActor, IController> CreateWalkingBot()
{
var step = 10 * Math.Sqrt(2);
var startPoint = new Frame3D(70, 70, 0);
return(actor => new SlardarBot(new List <Frame3D>
{
startPoint,
new Frame3D(startPoint.X, -startPoint.Y, 0),
}, actor, SlardarId.LeftTop));
}
