/// <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;
}
/// <summary>
/// Update unit speed based on wheels speed
/// </summary>
/// <param name="currentTime"></param>
public void UpdateSpeed(double currentTime)
{
if (movement == null)
{
return;
}
var engine = actor.World.GetEngine <ICommonEngine>();
var requestedAngular = rules.WheelRadius * (movement.RightRotatingVelocity.Radian - movement.LeftRotatingVelocity.Radian) / rules.DistanceBetweenWheels;
var requestedLinear = rules.WheelRadius * (movement.LeftRotatingVelocity.Radian + movement.RightRotatingVelocity.Radian) / 2;
var linear = requestedLinear; // Math.Sign(requestedLinear) * Math.Min(Math.Abs(requestedLinear), rules.LinearVelocityLimit);
var angular = requestedAngular; // Math.Sign(requestedAngular) * Math.Min(Math.Abs(requestedAngular), rules.AngularVelocityLimit.Radian);
var angle = engine.GetAbsoluteLocation(actor.ObjectId).Yaw.Radian;
//convert into cvarc world velocity
var unitSpeed = new AIRLab.Mathematics.Frame3D(
linear * Math.Cos(angle),
linear * Math.Sin(angle),
0,
Angle.Zero,
Angle.FromRad(angular),
Angle.Zero);
engine.SetAbsoluteSpeed(actor.ObjectId, unitSpeed);
EncodersData encoderRecord = new EncodersData
{
Timestamp = actor.World.Clocks.CurrentTime,
TotalLeftRotation = Angle.FromRad(movement.LeftRotatingVelocity.Radian * 0.005),
TotalRightRotation = Angle.FromRad(movement.RightRotatingVelocity.Radian * 0.005)
};
actor.DWMData.EncodersHistory.Add(encoderRecord);
}
/// <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));
}
/// <summary>
/// Update unit speed based on wheels speed
/// </summary>
/// <param name="currentTime"></param>
public void UpdateSpeed(double currentTime)
{
if (movement == null) return;
var requestedAngular = rules.WheelRadius * (movement.RightRotatingVelocity.Radian - movement.LeftRotatingVelocity.Radian) / rules.DistanceBetweenWheels;
var requestedLinear = rules.WheelRadius * (movement.LeftRotatingVelocity.Radian + movement.RightRotatingVelocity.Radian) / 2;
var linear = requestedLinear; // Math.Sign(requestedLinear) * Math.Min(Math.Abs(requestedLinear), rules.LinearVelocityLimit);
var angular = requestedAngular;// Math.Sign(requestedAngular) * Math.Min(Math.Abs(requestedAngular), rules.AngularVelocityLimit.Radian);
var angle = actor.World.Engine.GetAbsoluteLocation(actor.ObjectId).Yaw.Radian;
//convert into cvarc world velocity
var unitSpeed = new AIRLab.Mathematics.Frame3D(
linear * Math.Cos(angle),
linear * Math.Sin(angle),
0,
Angle.Zero,
Angle.FromRad(angular),
Angle.Zero);
actor.World.Engine.SetSpeed(actor.ObjectId, unitSpeed);
EncodersData encoderRecord = new EncodersData
{
Timestamp = actor.World.Clocks.CurrentTime,
TotalLeftRotation = Angle.FromRad(movement.LeftRotatingVelocity.Radian * 0.005),
TotalRightRotation = Angle.FromRad(movement.RightRotatingVelocity.Radian * 0.005)
};
actor.DWMData.EncodersHistory.Add(encoderRecord);
}
/// <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;
}
/// <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;
}
public Settings()
{
ObserverCameraLocation = new Frame3D(0, 0, 150, -Angle.HalfPi, Angle.Zero, -Angle.HalfPi);
Controllers = new List<ControllerSettings>();
Port = 14000;
SolutionsFolder = "Solutions";
OperationalTimeLimit = 1;
}
/// <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 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>
/// Создает камеру, которую можно крутить движением мышки
/// </summary>
/// <param name="eventSource">Control,с которого обрабатываются события </param>
/// <param name="cameraLocation">Исходное положение камеры</param>
public TrackballCamera(Control eventSource,Frame3D cameraLocation)
: base(eventSource.ClientSize.Width/(double) eventSource.ClientSize.Height)
{
EventSource = eventSource;
Vector3 tempLoc = cameraLocation.ToDirectXVector();
_cameraLocation = new Vector3(Math.Abs(tempLoc.X), 0, tempLoc.Z);
Angle ang = Geometry.Atan2(tempLoc.Y, tempLoc.X);
_rotationMatrix = Matrix.RotationZ(-(float) ang.Radian);
_radius = _cameraLocation.Length();
Scale = 1;
}
private void Rotate(int rotateDestination)
{
var angular = rotateDestination * rules.RotationAngle.Radian;
var speed = new Frame3D(
0,
0,
0,
Angle.Zero,
Angle.FromRad(angular),
Angle.Zero);
actor.World.Engine.SetSpeed(actor.ObjectId, speed);
}
public Frame3D Apply(Frame3D arg)
{
if (IsIdentity)
{
return(arg);
}
if (arg.IsIdentity)
{
return(this);
}
return(FromMatrix(FastMatrixMultiply(UniMatrix, arg.UniMatrix)));
}
public static void ProcessCommand(this IEngine engine, string actor, Command cmd)
{
if (cmd.Action == CommandAction.WaitForExit)
cmd.Time = 100000;
var location = engine.GetAbsoluteLocation(actor);
var speed = new Frame3D(cmd.LinearVelocity * Math.Cos(location.Yaw.Radian),
cmd.LinearVelocity * Math.Sin(location.Yaw.Radian), 0, Angle.Zero, cmd.AngularVelocity,
Angle.Zero);
engine.SetSpeed(actor, speed);
if (cmd.Action != CommandAction.None)
engine.PerformAction(actor, cmd.Action.ToString());
}
public SwitchableCamera(
Body sourceBody, Frame3D bodyOffset, Control form, Frame3D location)
: base(form.ClientSize.Width/(double) form.ClientSize.Height)
{
double aspectRatio = form.ClientSize.Width/(double) form.ClientSize.Height;
AddMode(ViewModes.Top, new TopViewCamera(location, aspectRatio));
AddMode(ViewModes.Trackball, new TrackballCamera(form, location));
if (sourceBody != null)
AddMode(ViewModes.FirstPerson,
new FirstPersonCamera(sourceBody, bodyOffset,
SceneConfig.FirstPersonViewAngle, aspectRatio));
}
public void Location()
{
var world = new Body();
var loc = new Frame3D(10, 0, 0);
world.Add(new Box {Location = loc});
Body box = world.Nested.First();
Assert.AreEqual(loc, box.Location);
Assert.AreEqual(loc, box.GetAbsoluteLocation());
box.Add(new Ball {Location = new Frame3D(0, 0, 30)});
Body ball = box.Nested.First();
Assert.AreEqual(new Frame3D(0, 0, 30), ball.Location);
Assert.AreEqual(new Frame3D(10, 0, 30), ball.GetAbsoluteLocation());
}
public Clapperboard(Frame3D location, Color color, string type)
{
XSize = 18;
YSize = 5;
ZSize = 18;
DefaultColor = Color.Black;
Location = location;
IsStatic = true;
NewId = type;
Cap = BuildCap(color);
Add(Cap);
}
public RMCombinedUnit(IRMRobot actor, RMWorld world)
: base(actor)
{
LeftClapperOffset = new Frame3D(0, 12, 6);
RightClapperOffset = new Frame3D(0, -12, 6);
this.world = world;
this.robot = actor;
SubUnits.Add("LeftDeployer", x => MakeClapper(x, LeftClapperOffset));
SubUnits.Add("RightDeployer", x => MakeClapper(x, RightClapperOffset));
SubUnits.Add("LadderTaker", x => GoStair(x));
SubUnits.Add("PopcornMachineUser", x => TakePopCorn(x));
}
private void Move()
{
var location = actor.World.Engine.GetAbsoluteLocation(actor.ObjectId);
var linear = rules.MovementRange/rules.MovementTime;
var angle = location.Yaw.Radian;
var speed = new Frame3D(
linear * Math.Cos(angle),
linear * Math.Sin(angle),
0,
Angle.Zero,
Angle.Zero,
Angle.Zero);
actor.World.Engine.SetSpeed(actor.ObjectId, speed);
}
private void SaveLocation(Frame3D newLocation, double totalTime)
{
if(newLocation.Equals(_lastLocation))
{
_isCurrentlyMoving = false;
return;
}
if(!_isCurrentlyMoving)
{
Movements.Add(new Movement(totalTime));
_isCurrentlyMoving = true;
}
_lastLocation = newLocation;
Movements.Last().SaveLocation(newLocation);
}
public void TreeMovingNoParent()
{
var oldBoxLocation = new Frame3D(10, 20, 30);
var box = new Box
{
Location = oldBoxLocation
};
var newParent = new Body
{
Location = new Frame3D(20, 10, 20)
};
newParent.DetachAttachMaintaingLoction(box);
Assert.AreEqual(oldBoxLocation, box.GetAbsoluteLocation());
Assert.AreEqual(new Frame3D(-10, 10, 10), box.Location);
}
public void TreeMovingConstantLocation()
{
var world = new Body();
var boxLocation = new Frame3D(10, 0, 0);
var ballLocation = new Frame3D(20, 10, 0);
var box = new Box {Location = boxLocation};
var ball = new Ball {Location = ballLocation};
world.Add(box);
world.Add(ball);
box.DetachAttachMaintaingLoction(ball);
Assert.AreEqual(boxLocation, box.Location);
Assert.AreEqual(ballLocation, ball.GetAbsoluteLocation());
Assert.AreEqual(new Frame3D(10, 10, 0), ball.Location);
world.DetachAttachMaintaingLoction(ball);
Assert.AreEqual(ballLocation, ball.Location);
Assert.AreEqual(ballLocation, ball.GetAbsoluteLocation());
}
public void Attach(string objectToAttach, string host, Frame3D relativePosition)
{
var parent = GameObject.Find(host);
var attachment = GameObject.Find(objectToAttach);
// move attachment to (0, 0, 0) relative to parent
attachment.transform.position = parent.transform.position;
attachment.transform.rotation = parent.transform.rotation;
// set attachments position and rotation relative to parent
var rp = relativePosition;
attachment.transform.position += Quaternion.Euler(parent.transform.eulerAngles) *
new Vector3((float)rp.X, (float)rp.Z, (float)rp.Y);
attachment.transform.rotation *= Quaternion.Euler((float)rp.Roll.Grad, (float)rp.Yaw.Grad, (float)rp.Pitch.Grad);
// create unbreakable joint between attachment and parent
var joint = attachment.AddComponent<FixedJoint>();
joint.connectedBody = parent.GetComponent<Rigidbody>();
joint.enableCollision = false;
joint.breakForce = Single.PositiveInfinity;
attachedParams.Add(attachment, new Tuple<float, float>(attachment.GetComponent<Rigidbody>().drag, attachment.GetComponent<Rigidbody>().angularDrag));
attachment.GetComponent<Rigidbody>().drag = attachment.GetComponent<Rigidbody>().angularDrag = 0;
// Второй способ: приаттачивание с помощью родительского трансформа.
// Не стоит использовать, т.к. юнька при аттаче localScale ребенка
// становится зависимым от localScale родителя.
//// physics no affects the attachments rigidbody
//if (attachment.rigidbody != null)
// attachment.rigidbody.isKinematic = true;
//// move attacment to (0, 0, 0) relative to parent transform position
//attachment.transform.position = parent.transform.position;
//attachment.transform.rotation = parent.transform.rotation;
//// set parent
//attachment.transform.parent = parent.transform;
//// set attachments position and rotation relative to parent
//var rp = relativePosition;
//attachment.transform.localPosition = new Vector3((float)rp.X / parent.transform.localScale.x,
// (float)rp.Z / parent.transform.localScale.y,
// (float)rp.Y / parent.transform.localScale.z);
//attachment.transform.localRotation = Quaternion.Euler((float)rp.Roll.Grad, (float)rp.Yaw.Grad, (float)rp.Pitch.Grad);
}
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 double MakeClapper(IActor actor, Frame3D deployerOffset)
{
Debugger.Log(RMDebugMessage.Logic, "Closing clapperboard");
var actorLocation = world.Engine.GetAbsoluteLocation(actor.ObjectId);
var deployerLocation = GetDirectionFrame(actorLocation).Apply(deployerOffset) + actorLocation;
var target = GetClapperboardToDeploy(actor, deployerLocation);
if (target == null)
{
Debugger.Log(RMDebugMessage.Logic, "Clapperboard not found!");
return 1;
}
world.Manager.CloseClapperboard(target.Item1);
world.ClosedClapperboards.Add(target.Item1);
SolveClapperboardScores(actor, target.Item2 == SideColor.Yellow ? TwoPlayersId.Left : TwoPlayersId.Right);
Debugger.Log(RMDebugMessage.Logic, "Clapperboard closed");
return 1;
}
void ApplyCommand(double time)
{
if (currentMovement == null) return;
var location = actor.World.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);
actor.World.Engine.SetSpeed(actor.ObjectId, requestedSpeed);
}
public void Attach(string objectToAttach, string host, Frame3D relativePosition)
{
var parent = GameObject.Find(host);
var attachment = GameObject.Find(objectToAttach);
// move attachment to (0, 0, 0) relative to parent
attachment.transform.position = parent.transform.position;
attachment.transform.rotation = parent.transform.rotation;
// set attachments position and rotation relative to parent
var rp = relativePosition;
attachment.transform.position += Quaternion.Euler(parent.transform.eulerAngles) *
new Vector3((float)rp.X, (float)rp.Z, (float)rp.Y);
attachment.transform.rotation *= Quaternion.Euler((float)rp.Roll.Grad, (float)rp.Yaw.Grad, (float)rp.Pitch.Grad);
// create unbreakable joint between attachment and parent
var joint = attachment.AddComponent<FixedJoint>();
joint.connectedBody = parent.GetComponent<Rigidbody>();
joint.enableCollision = false;
joint.breakForce = Single.PositiveInfinity;
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>
/// Присоединит переданный объект, используя переданный 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);
}
}
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);
}
public static double Hypot(Frame3D frame)
{
return(Hypot(frame.X, frame.Y, frame.Z));
}
public Point3D Apply(Point3D arg)
{
Frame3D r = Apply(arg.ToFrame());
return(new Point3D(r.X, r.Y, r.Z));
}
public void Detach(string objectToDetach, Frame3D absolutePosition)
{
var attachment = GameObject.Find(objectToDetach);
var joints = attachment.GetComponents<FixedJoint>();
foreach(var joint in joints)
GameObject.Destroy(joint);
if (attachedParams.ContainsKey(attachment))
{
var attachmentParams = attachedParams[attachment];
attachment.GetComponent<Rigidbody>().drag = attachmentParams.Item1;
attachment.GetComponent<Rigidbody>().angularDrag = attachmentParams.Item2;
attachedParams.Remove(attachment);
}
}
public void SetSpeed(string id, Frame3D speed)
{
requested[id] = new Frame2D(speed.X, speed.Y, -speed.Yaw);
}
public bool Equals(Frame3D other)
{
return(other.X.Equals(X) && other.Y.Equals(Y) && other.Z.Equals(Z) &&
other.Pitch.Radian.Equals(Pitch.Radian) &&
other.Yaw.Radian.Equals(Yaw.Radian) && other.Roll.Radian.Equals(Roll.Radian));
}
private static void CheckFrameIsWithin(Frame3D expected, Frame3D actual, double epsilon = 0.01)
{
Assert.Less(Geometry.Hypot(expected-actual),epsilon, "Expected {0} but was {1}", expected,actual);
}
