void FixedUpdate()
{
if (evaluation.IsComplete)
{
return;
}
if (Network != null)
{
var upperTheta = AngleHelper.GetAngle(upper.rotation);
var lowerTheta = AngleHelper.GetAngle(lower.rotation);
var position = wheel.transform.localPosition.x;
Network.Clear();
Network.UpperTheta.Set(upperTheta);
Network.LowerTheta.Set(lowerTheta);
Network.Position.Set(position);
Network.Tick();
this.speed = (float)Network.Speed.Get();
// Update motor speed
SetMotorSpeed(this.speed);
}
}
public void CalcularAngulosFinales()
{
angulos = AngleHelper.SetValorAngulos(HombroDerecho,
ManoDerecha, CodoDerecho, skelObjeto);
if (angulos[0] == -1 || angulos[1] == -1 || angulos[2] == -1 || angulos[3] == -1)
{
//Console.WriteLine("no se puede alcanzar el objeto");
/*dc.DrawText(
* new FormattedText("No se puede alcanzar el objeto",
* CultureInfo.GetCultureInfo("en-us"),
* FlowDirection.LeftToRight,
* new Typeface("Verdana"),
* 25, System.Windows.Media.Brushes.Red),
* new Point(0,0));*/
flagSkeleton = false;
}
else
{
flagSkeleton = true;
//si se puede alcanzar el objeto,
try
{
this.ConfirmacionButton.IsEnabled = true;
this.MensajesLabel.Content = "Angulos calculados";
}
catch (Exception)
{
Console.WriteLine("Excepcion en escribir el angulo en arduino");
}
}
}
public static RESULT Rotate(float currentAngleY, float targetAngleY, float speed, float arriveThreshold)
{
float dAngle = targetAngleY - currentAngleY;
dAngle = AngleHelper.LimitAngleInNPI_PI(dAngle);
float turnLimitAngle = speed * TimeHelper.deltaTime;
float cAngle = 0;
if (dAngle > 0)
{
cAngle = Mathf.Min(turnLimitAngle, dAngle);
}
else if (dAngle < 0)
{
cAngle = Mathf.Max(-turnLimitAngle, dAngle);
}
float leftDegree = AngleHelper.LimitAngleInNPI_PI(dAngle - cAngle);
RESULT r = new RESULT();
r.arrived = Mathf.Abs(leftDegree) <= arriveThreshold;
r.turnDegree = cAngle;
r.leftDegree = leftDegree;
r.destinationAngleY = currentAngleY + cAngle;
return(r);
}
public static void UpdateVision(Bot bot, IEnumerable <Bot> allBots)
{
foreach (var e in bot.Eyes)
{
e.Clear();
}
foreach (var otherBot in allBots.Where(b => b != bot))
{
var distanceVector = otherBot.Position - bot.Position;
var distanceSquared = distanceVector.LengthSquared();
if (distanceSquared < VisionLimitSquared)
{
// Take the bearing to the centre of the other bot
var angleToOtherBot = (float)Math.Atan2(distanceVector.Y, distanceVector.X);
var relativeAngle = AngleHelper.NormaliseAngle(angleToOtherBot - bot.Orientation);
// Translate to within -pi to +pi (move the discontinuity to outside the edges of vision)
if (relativeAngle > Math.PI)
{
relativeAngle -= 2 * (float)Math.PI;
}
// Calculate what arc the other bot should fill
var distance = Math.Sqrt(distanceSquared);
var radiusAngle = Math.Atan2(otherBot.Radius, distance);
var visibleAngleStart = relativeAngle - radiusAngle;
var visibleAngleStop = relativeAngle + radiusAngle;
for (var i = 0; i < Bot.EyeCount; i++)
{
var eyePosition = i - Bot.EyeCount / 2;
var eyeStart = (eyePosition * Bot.EyeAngle * 2) - Bot.EyeAngle;
var eyeStop = (eyePosition * Bot.EyeAngle * 2) + Bot.EyeAngle;
if (
visibleAngleStart > eyeStart && visibleAngleStart < eyeStop || // Other bot start edge is in the eye
visibleAngleStop > eyeStart && visibleAngleStop < eyeStop || // Other bot stop edge is in the eye
visibleAngleStart < eyeStart && visibleAngleStop > eyeStop // Other bot completely covers the eye
)
{
bot.Eyes[i].Add(new EyeEntry(otherBot, (float)distance));
}
}
}
}
bot.EyeDistances = bot.Eyes.Select(e => e.Any() ? Bot.VisionLimit - e.Min(n => n.Distance) : 0).ToArray();
var middleEye = bot.Eyes[bot.FocusEye];
if (middleEye.Any())
{
bot.FocussedBot = middleEye.OrderBy(e => e.Distance).First().OtherBot;
}
else
{
bot.FocussedBot = null;
}
}
public void NormaliseAngleReducesVeryPositiveAngle()
{
const float TestAngle = 2.0f;
var result = AngleHelper.NormaliseAngle(TestAngle + (float)Math.PI * 6);
result.Should().BeApproximately(TestAngle, AngleTolerance);
}
public void NormaliseAngleLeavesSmallAngle()
{
const float TestAngle = 2.0f;
var result = AngleHelper.NormaliseAngle(TestAngle);
result.Should().BeApproximately(TestAngle, AngleTolerance);
}
public void NormaliseAngleIncreasesVeryNegativeAngle()
{
const float TestAngle = 2.0f;
var result = AngleHelper.NormaliseAngle(TestAngle - (float)Math.PI * 6);
result.Should().BeApproximately(TestAngle, AngleTolerance);
}
private double CalcInterceptAngle(Orbit targetOrbit, Orbit originOrbit)
{
double originRadius = (originOrbit.semiMinorAxis + originOrbit.semiMajorAxis) * 0.5;
double targetRadius = (targetOrbit.semiMinorAxis + targetOrbit.semiMajorAxis) * 0.5;
double angle = 180.0 * (1.0 - Math.Pow((originRadius + targetRadius) / (2.0 * targetRadius), 1.5));
angle = PhaseAngle - angle;
return(RelativeInclination < 90.0 ? AngleHelper.Clamp360(angle) : AngleHelper.Clamp360(360.0 - (180.0 - angle)));
}
private void CursorHandle()
{
var position = new Vector3(Input.mousePosition.x, Input.mousePosition.y, 1);
position = Camera.main.ScreenToWorldPoint(position);
var projectileVector = new Vector2(position.x - transform.position.x, position.y - transform.position.y);
gameObject.GetComponent <Transform>().rotation = Quaternion.Euler(0, 0, AngleHelper.GetAngleForParticles(projectileVector) - arc / 2);
}
// Token: 0x06000A47 RID: 2631 RVA: 0x0002C8EA File Offset: 0x0002ACEA
public static float LimitAngleInNPI_PI(float angle)
{
angle = AngleHelper.LimitAngleIn0_360(angle);
if (angle > 180f)
{
angle -= 360f;
}
return(angle);
}
private void LookingAngle()
{
Vector3 pz = Camera.main.ScreenToWorldPoint(Input.mousePosition);
Vector2 mouse = new Vector2(pz.x, pz.y);
Vector2 vectorFromCenter = new Vector2(mouse.x - rb.position.x, mouse.y - rb.position.y);
double angle = AngleHelper.GetAngleForTurningAround(vectorFromCenter);
AnalyzeAngle(angle);
}
public static float CalcRotationTo(Vector3 a, Vector3 b, float currentAngleY)
{
float targetAngle = CalcRotationAngleY(a, b);
float dAngle = (targetAngle - currentAngleY);
dAngle = AngleHelper.LimitAngleInNPI_PI(dAngle);
return(dAngle);
}
public override void Draw(SectionModule section)
{
var target = FlightGlobals.fetch.VesselTarget?.GetVessel();
if (target != null)
{
double latitude = AngleHelper.Clamp360(target.latitude);
DrawLine(Units.ToAngleDMS(latitude) + (latitude < 0 ? " S" : " N"), section.IsHud);
}
}
public override void Draw(SectionModule section)
{
var target = FlightGlobals.fetch.VesselTarget;
if (target != null)
{
var vessel = target.GetVessel();
double longitude = AngleHelper.Clamp180(vessel.longitude);
DrawLine(Units.ToAngleDMS(longitude) + (longitude < 0.0 ? " W" : " E"), section.IsHud);
}
}
public ParticleGenerator(Vector2 position, Dictionary <string, int> spritePercentage, Direction direction, Angle sprayAngle, float duration, float frequency, float particlesPerGeneration)
{
_x = position.X;
_y = position.Y;
_spritePercentage = spritePercentage;
Rotation = AngleHelper.DirectionToFloat(direction);
SprayAngle = sprayAngle;
Duration = duration;
Frequency = frequency;
ParticlesPerGeneration = particlesPerGeneration;
}
public static void UpdateBot(Bot bot)
{
bot.Orientation = AngleHelper.NormaliseAngle(bot.Orientation + AngleHelper.IntToAngle(bot.Memory[(int)MemoryAddresses.TurnRight] - bot.Memory[(int)MemoryAddresses.TurnLeft]));
// Right and down are positive x and y respectively
var netForce = new Vector2(
bot.GetFromMemory(MemoryAddresses.MoveUp) - bot.GetFromMemory(MemoryAddresses.MoveDown),
bot.GetFromMemory(MemoryAddresses.MoveRight) - bot.GetFromMemory(MemoryAddresses.MoveLeft));
var rotation = Matrix3x2.CreateRotation(bot.Orientation);
bot.Force += Vector2.Transform(netForce, rotation);
}
public void DrawSegment(SpriteBatch spriteBatch, ISegment segment)
{
Texture2D segmentTexture2D;
_textures.TryGetValue("SegmentTexture", out segmentTexture2D);
if (segmentTexture2D == null)
{
throw new Exception("ERROR : Texture not found in the dictionary");
}
float radianAngle = AngleHelper.SlopeToRadian((float)segment.Slope);
spriteBatch.Draw(segmentTexture2D, new Rectangle((int)segment.Point1.X, (int)segment.Point1.Y, (int)segment.Lenght + 1, 1), null, Color.Black, radianAngle, new Vector2(0, 0), SpriteEffects.None, 1f);
}
public void AngleToIntScalesCorrectly()
{
const float FullAngle = 2 * (float)Math.PI;
const int FullAngleInt = 1000;
var testAngle1 = AngleHelper.AngleToInt(FullAngle);
var testAngle2 = AngleHelper.AngleToInt(FullAngle / 2);
var testAngle3 = AngleHelper.AngleToInt(FullAngle / 3);
testAngle1.Should().Be(FullAngleInt);
testAngle2.Should().Be(FullAngleInt / 2);
testAngle3.Should().Be(FullAngleInt / 3);
}
public void IntToAngleScalesCorrectly()
{
const float FullAngle = 2 * (float)Math.PI;
const int FullAngleInt = 1000;
var testAngle1 = AngleHelper.IntToAngle(FullAngleInt);
var testAngle2 = AngleHelper.IntToAngle(FullAngleInt / 2);
var testAngle3 = AngleHelper.IntToAngle(FullAngleInt / 3);
testAngle1.Should().BeApproximately(FullAngle, AngleTolerance);
testAngle2.Should().BeApproximately(FullAngle / 2, AngleTolerance);
testAngle3.Should().BeApproximately(FullAngle / 3, AngleTolerance);
}
private void Movement()
{
var position = Character.transform.position;
var vectorFromCenter = new Vector2(position.x - transform.position.x, position.y - transform.position.y);
double angle = AngleHelper.GetAngleForTurningAround(vectorFromCenter);
var angleFromSectorBeginning = angle + (Constants.Round / NumOfSpriteTurns) / 2;
int spriteNr = (int)(angleFromSectorBeginning / (Constants.Round / NumOfSpriteTurns));
if (spriteNr < 0 || spriteNr == NumOfSpriteTurns)
{
spriteNr = 0;
}
}
void FixedUpdate()
{
if (isComplete)
{
return;
}
// Consider 30 seconds to be solved
var duration = Time.time - startTime;
if (duration > 30.0f)
{
EndTrial();
}
// End if it fell off the edge
if (wheel.transform.position.y - startPosition.y < -2.0f)
{
EndTrial();
return;
}
var thetaLower = AngleHelper.GetAngle(lower.rotation);
var thetaDotLower = AngleHelper.GetAngle(lower.angularVelocity);
var thetaUpper = AngleHelper.GetAngle(upper.rotation);
var thetaDotUpper = AngleHelper.GetAngle(upper.angularVelocity);
var x = wheel.transform.localPosition.x;
var xDot = wheel.velocity.magnitude; // speed?
// End if it went out of bounds
if (Mathf.Abs(x) > 14.0f)
{
EndTrial();
return;
}
var upperFell = NumberHelper.Between(Mathf.Abs(thetaUpper), 110.0f, 250.0f);
var lowerFell = NumberHelper.Between(Mathf.Abs(thetaLower), 110.0f, 250.0f);
if (upperFell || lowerFell)
{
EndTrial();
return;
}
currentTrial.Update(thetaLower, thetaDotLower,
thetaUpper, thetaDotUpper,
x, xDot);
}
public void TestGetAngle()
{
Assert.AreEqual(0.0f, AngleHelper.GetAngle(-360.0f));
Assert.AreEqual(0.0f, AngleHelper.GetAngle(0.0f));
Assert.AreEqual(0.0f, AngleHelper.GetAngle(360.0f));
Assert.AreEqual(90.0f, AngleHelper.GetAngle(-270.0f));
Assert.AreEqual(90.0f, AngleHelper.GetAngle(90.0f));
Assert.AreEqual(-90.0f, AngleHelper.GetAngle(-90.0f));
Assert.AreEqual(-90.0f, AngleHelper.GetAngle(270.0f));
Assert.AreEqual(180.0f, AngleHelper.GetAngle(180.0f));
Assert.AreEqual(-180.0f, AngleHelper.GetAngle(-180.0f));
}
public static Vector3 CalcTileArrowOffset(Spine.Slot boundingSlot, Transform transform, float rotation,
float innerRate = 0.7f)
{
float radius = AngleHelper.AngleToRadius(rotation - 90);
Vector3 v = new Vector3(Mathf.Cos(radius), Mathf.Sin(radius)) * 10000;
Spine.BoundingBoxAttachment bounding = boundingSlot.Attachment as Spine.BoundingBoxAttachment;
float[] vertices = (float[])bounding.Vertices.Clone();
Vector2 p1 = new Vector2();
Vector2 p2 = new Vector2();
int n = vertices.Length / 2;
for (int i = 0; i <= n; ++i)
{
int m = i % n;
float localX = vertices[m * 2 + 0];
float localY = vertices[m * 2 + 1];
p2.Set(localX, localY);
if (i >= 1)
{
float intersectDist;
Vector2 intersectPoint;
if (GeometryHelper.CalcIntersectPoint(Vector2.zero, v, p1, p2, out intersectPoint, out intersectDist))
{
intersectPoint *= innerRate;
float worldX;
float worldY;
boundingSlot.Bone.localToWorld(intersectPoint.x, intersectPoint.y, out worldX, out worldY);
Vector3 offset = new Vector3(worldX, worldY, 0);
offset = transform.TransformPoint(offset);
return(offset);
}
}
p1 = p2;
}
Vector3 boneCenter = new Vector3(boundingSlot.Bone.WorldX, boundingSlot.Bone.WorldY, 0);
boneCenter = transform.TransformPoint(boneCenter);
return(boneCenter);
}
public PruebaAngulos()
{
mano = new Joint();
mano.Position = SetPosition(-0.87, 4.29, 1.89);
codo = new Joint();
codo.Position = SetPosition(-0.87, 2.31, 1.8);
hombro = new Joint();
hombro.Position = SetPosition(-0.87, 0.52, 3.8);
objeto = new SkeletonPoint();
objeto = SetPosition(-0.87, 4.74, 4.1);
AngleHelper.SetValorAngulos(hombro, mano, codo, objeto);
}
public void UpdateBotSetsRotation()
{
const int turnLeft = 100;
const int turnRight = 350;
var bot = new Bot
{
Orientation = 0
};
bot.SetMemory(MemoryAddresses.TurnLeft, turnLeft);
bot.SetMemory(MemoryAddresses.TurnRight, turnRight);
RuntimeEngine.UpdateBot(bot);
bot.Orientation.Should().BeApproximately(AngleHelper.IntToAngle(turnRight - turnLeft), 3);
}
protected void ChangeAnimation(Vector2 vectorFromCenter)
{
double angle = AngleHelper.GetAngleForTurningAround(vectorFromCenter);
var angleFromSectorBeginning = angle + (Constants.Round / NumOfSpriteTurns) / 2;
int spriteNr = (int)(angleFromSectorBeginning / (Constants.Round / NumOfSpriteTurns));
if (spriteNr < 0 || spriteNr == NumOfSpriteTurns)
{
spriteNr = 0;
}
if (PrevBehaviour == spriteNr)
{
return;
}
Animator.SetInteger("angle", spriteNr); //throwing warning - need to expect this
PrevBehaviour = spriteNr;
}
public PruebaAngulos()
{
hombro = new Joint();
hombro.Position = SetPosition(2.3, 1.21, -1.67);
codo = new Joint();
codo.Position = SetPosition(2.3, 0.98, -0.78);
mano = new Joint();
mano.Position = SetPosition(2.3, 1.42, 0.3);
objeto = new SkeletonPoint();
objeto = SetPosition(2.3, 0.25, -0.17);
AngleHelper a = new AngleHelper();
a.SetValorAngulos(new Puntos(hombro, codo, mano, objeto));
}
public ProjectedPoint FindMarkRoundPoint(Mark previousMark, Mark nextMark)
{
ProjectedPoint myA = AveragedLocation.Project();
float previousAngle = (float)AngleHelper.FindAngleWTF(previousMark.AveragedLocation.Project(), myA);
previousAngle = AngleHelper.NormalizeAngle(-previousAngle);
float nextAngle = (float)AngleHelper.FindAngleWTF(nextMark.AveragedLocation.Project(), myA);
nextAngle = AngleHelper.NormalizeAngle(-nextAngle);
float roundingAngle = AngleHelper.FindHalfwayCounterClockwiseAngle(previousAngle, nextAngle);
ProjectedPoint pp = AngleHelper.PolarToRectangular(myA, (double)roundingAngle, 1000.0);
return(pp);
}
public override void Draw(SectionModule section)
{
var target = FlightGlobals.fetch.VesselTarget;
if (target != null)
{
var vessel = target.GetVessel();
if (vessel == null)
{
DrawLine("N/A", section.IsHud);
}
else
{
double latitude = AngleHelper.Clamp180(vessel.latitude);
DrawLine(Units.ToAngleDMS(latitude) + (latitude < 0 ? " S" : " N"), section.IsHud);
}
}
}
public override void Draw(Unity.Flight.ISectionModule section)
{
var target = Rendezvous.RendezvousProcessor.targetVessel;
if (target != null)
{
var vessel = target.GetVessel();
if (vessel == null)
{
DrawLine("N/A", section.IsHud);
}
else
{
double latitude = AngleHelper.Clamp180(vessel.latitude);
DrawLine(Units.ToAngleDMS(latitude) + (latitude < 0 ? " S" : " N"), section.IsHud);
}
}
}
