public Vector3 ToEulerAnglesXYZ()
{
Real yAngle;
Real rAngle;
Real pAngle;
pAngle = Utility.ASin(m01);
if (pAngle < Utility.PI / 2)
{
if (pAngle > -Utility.PI / 2)
{
yAngle = Utility.ATan2(m21, m11);
rAngle = Utility.ATan2(m02, m00);
}
else
{
// WARNING. Not a unique solution.
Real fRmY = (Real)Utility.ATan2(-m20, m22);
rAngle = 0.0f; // any angle works
yAngle = rAngle - fRmY;
}
}
else
{
// WARNING. Not a unique solution.
Real fRpY = Utility.ATan2(-m20, m22);
rAngle = 0.0f; // any angle works
yAngle = fRpY - rAngle;
}
return(new Vector3(yAngle, rAngle, pAngle));
}
public void ToEulerAngles(out Real pitch, out Real yaw, out Real roll)
{
Real halfPi = Utility.PI / 2;
Real test = x * y + z * w;
if (test > 0.499f)
{
// singularity at north pole
yaw = 2 * Utility.ATan2(x, w);
roll = halfPi;
pitch = 0;
}
else if (test < -0.499f)
{
// singularity at south pole
yaw = -2 * Utility.ATan2(x, w);
roll = -halfPi;
pitch = 0;
}
else
{
Real sqx = x * x;
Real sqy = y * y;
Real sqz = z * z;
yaw = Utility.ATan2(2 * y * w - 2 * x * z, 1 - 2 * sqy - 2 * sqz);
roll = (Real)Utility.ASin(2 * test);
pitch = Utility.ATan2(2 * x * w - 2 * y * z, 1 - 2 * sqx - 2 * sqz);
}
if (pitch >= -Real.Epsilon && pitch <= Real.Epsilon)
{
pitch = 0f;
}
else if (pitch == -Utility.PI)
{
pitch = Utility.PI;
}
if (yaw >= -Real.Epsilon && yaw <= Real.Epsilon)
{
yaw = 0f;
}
else if (yaw == -Utility.PI)
{
yaw = Utility.PI;
}
if (roll >= -Real.Epsilon && roll <= Real.Epsilon)
{
roll = 0f;
}
else if (roll == -Utility.PI)
{
roll = Utility.PI;
}
}
public Radian GetRotation(Vector2 dst)
{
if (this == dst)
{
return(Utility.ATan2(0, 0));
}
var diff = (dst - this).ToNormalized();
return(Utility.ATan2(diff.Y, diff.X));
}
public static Radian GetRotation(this OpenTK.Vector2 left, OpenTK.Vector2 right)
{
if (left == right)
{
return(Utility.ATan2(0, 0));
}
var diff = (right - left).ToNormalized();
return(Utility.ATan2(diff.Y, diff.X));
}
public void ToEulerAngles(out Real pitch, out Real yaw, out Real roll)
{
var halfPi = Utility.PI / 2;
var test = this.x * this.y + this.z * this.w;
if (test > 0.499f)
{
// singularity at north pole
yaw = 2 * Utility.ATan2(this.x, this.w);
roll = halfPi;
pitch = 0;
}
else if (test < -0.499f)
{
// singularity at south pole
yaw = -2 * Utility.ATan2(this.x, this.w);
roll = -halfPi;
pitch = 0;
}
else
{
var sqx = this.x * this.x;
var sqy = this.y * this.y;
var sqz = this.z * this.z;
yaw = Utility.ATan2(2 * this.y * this.w - 2 * this.x * this.z, 1 - 2 * sqy - 2 * sqz);
roll = (Real)Utility.ASin(2 * test);
pitch = Utility.ATan2(2 * this.x * this.w - 2 * this.y * this.z, 1 - 2 * sqx - 2 * sqz);
}
if (pitch <= Real.Epsilon)
{
pitch = 0f;
}
if (yaw <= Real.Epsilon)
{
yaw = 0f;
}
if (roll <= Real.Epsilon)
{
roll = 0f;
}
}
public void ToEulerAngles(out float pitch, out float yaw, out float roll)
{
float halfPi = Utility.PI / 2;
float test = x * y + z * w;
if (test > 0.499f)
{
// singularity at north pole
yaw = 2 * Utility.ATan2(x, w);
roll = halfPi;
pitch = 0;
}
else if (test < -0.499f)
{
// singularity at south pole
yaw = -2 * Utility.ATan2(x, w);
roll = -halfPi;
pitch = 0;
}
else
{
float sqx = x * x;
float sqy = y * y;
float sqz = z * z;
yaw = Utility.ATan2(2 * y * w - 2 * x * z, 1 - 2 * sqy - 2 * sqz);
roll = (float)Utility.ASin(2 * test);
pitch = Utility.ATan2(2 * x * w - 2 * y * z, 1 - 2 * sqx - 2 * sqz);
}
if (pitch <= float.Epsilon)
{
pitch = 0f;
}
if (yaw <= float.Epsilon)
{
yaw = 0f;
}
if (roll <= float.Epsilon)
{
roll = 0f;
}
}
public void line(Vector2 start, Vector2 end, Real width, Real rotation)
{
if (rotation != 0)
{
start = start.Rotate(Vector2.Zero, rotation);
end = end.Rotate(Vector2.Zero, rotation);
}
var diff = end - start;
//var srcRect = new AxisAlignedBox(start.X, start.Y - width * 0.5f, start.X + diff.Length(), start.Y + width * 0.5f);
var srcRect = new AxisAlignedBox2(start.X, start.Y, start.X + width, start.Y + diff.Length);
diff.Normalize();
var _rotation = Utility.ATan2(diff.Y, diff.X) - Utility.PIOverTwo;
//Draw(renderQueue, material, srcRect, AxisAlignedBox2.Null, color, rotation, new Vector2(0.5f, 0), SpriteEffects.None, depth);
var item = new GLRenderable();
item.color = _state.color;
item.depth = _state.depth;
item.material = _state.material;
item.pivot = new Vector2(0.5f, 0);
item.position = (_state.position + start) * _state.scale;
item.rotation = _rotation;
item.scale = srcRect.Size * _state.scale;
item.scissorRect = _clipping;
item.applyScissor = !_clipping.IsNull;
item.sourceRectangle = _state.source;
item.effect = _effect;
item.type = GLRenderableType.Line;
item.key = 0;
draw(item);
}
/// <summary>
/// Updates the state in accordance with movement instructions
/// </summary>
public override bool updateState(double delta)
{
double difference;
if (steerDelegate == null)
{ //Should we aim anyway?
if (bSkipAim)
{
difference = Helpers.calculateDifferenceInAngles(_state.yaw, angle);
if (Math.Abs(difference) < 2)
{
stopRotating();
}
else if (difference > 0)
{
rotateRight();
}
else
{
rotateLeft();
}
}
else
{
stopRotating();
}
stopThrusting();
stopStrafing();
steerDelegate = null;
return(base.updateState(delta));
}
vehicle.Position = _state.position();
//Determine our steering vector
Vector3 steer = steerDelegate(vehicle);
float degrees;
//Which direction are we supposed to be travelling in?
double yaw = Utility.ATan2(steer.x, steer.y);
degrees = -((Utility.RadiansToDegrees(yaw) / 1.5f) - 120);
double steerDifference = Helpers.calculateDifferenceInAngles(_state.yaw, degrees);
if (bSkipAim)
{
difference = Helpers.calculateDifferenceInAngles(_state.yaw, angle);
}
else
{
difference = steerDifference;
}
if (!bSkipRotate || bSkipAim)
{ //Turn to face our target
if (Math.Abs(difference) < 2)
{
stopRotating();
}
else if (difference > 0)
{
rotateRight();
}
else
{
rotateLeft();
}
}
else
{
stopRotating();
}
//Move appropriately towards the target
if (steer.Length < 0.6f)
{
stopThrusting();
stopStrafing();
}
else if (steerDifference > 90 || steerDifference < -90)
{
thrustBackward();
stopStrafing();
}
else if (steerDifference > 50)
{
stopThrusting();
strafeRight();
}
else if (steerDifference < -50)
{
stopThrusting();
strafeLeft();
}
else
{
thrustForward();
stopStrafing();
}
//These should be updated every poll
bSkipAim = false;
bSkipRotate = false;
steerDelegate = null;
return(base.updateState(delta));
}
public static float ToAngle(this Vector2 pos)
{
return(Utility.ATan2(pos.Y, pos.X));
}
public Radian GetRotation()
{
var diff = this.ToNormalized();
return(Utility.ATan2(diff.Y, diff.X));
}
public static Radian GetRotation(this OpenTK.Vector2 left)
{
var diff = left.ToNormalized();
return(Utility.ATan2(diff.Y, diff.X));
}
