public static int IntersectionCircleCircle(Circle c1, Circle c2, out Vector3D p1, out Vector3D p2)
{
p1 = new Vector3D();
p2 = new Vector3D();
// A useful page describing the cases in this function is:
// http://ozviz.wasp.uwa.edu.au/~pbourke/geometry/2circle/
p1.Empty();
p2.Empty();
// Vector and distance between the centers.
Vector3D v = c2.Center - c1.Center;
double d = v.Abs();
double r1 = c1.Radius;
double r2 = c2.Radius;
// Circle centers coincident.
if (Tolerance.Zero(d))
{
if (Tolerance.Equal(r1, r2))
{
return(-1);
}
else
{
return(0);
}
}
// We should be able to normalize at this point.
if (!v.Normalize())
{
Debug.Assert(false);
return(0);
}
// No intersection points.
// First case is disjoint circles.
// Second case is where one circle contains the other.
if (Tolerance.GreaterThan(d, r1 + r2) ||
Tolerance.LessThan(d, Math.Abs(r1 - r2)))
{
return(0);
}
// One intersection point.
if (Tolerance.Equal(d, r1 + r2) ||
Tolerance.Equal(d, Math.Abs(r1 - r2)))
{
p1 = c1.Center + v * r1;
return(1);
}
// There must be two intersection points.
p1 = p2 = v * r1;
double temp = (r1 * r1 - r2 * r2 + d * d) / (2 * d);
double angle = Math.Acos(temp / r1);
Debug.Assert(!Tolerance.Zero(angle) && !Tolerance.Equal(angle, Math.PI));
p1.RotateXY(angle);
p2.RotateXY(-angle);
p1 += c1.Center;
p2 += c1.Center;
return(2);
}
/// <summary>
/// Reflect ourselves about another sphere.
/// </summary>
public void Reflect(Sphere sphere)
{
// An interior point used to calculate whether we get inverted.
Vector3D interiorPoint;
if (IsPlane)
{
Debug.Assert(!this.Normal.IsOrigin);
interiorPoint = this.Offset - this.Normal;
}
else
{
// We don't want it to be the center, because that will reflect to infinity.
interiorPoint = (this.Center + new Vector3D(this.Radius / 2, 0));
}
if (Invert)
{
interiorPoint = ReflectPoint(interiorPoint);
}
Debug.Assert(IsPointInside(interiorPoint));
interiorPoint = sphere.ReflectPoint(interiorPoint);
Debug.Assert(!interiorPoint.DNE);
if (this.Equals(sphere))
{
if (IsPlane)
{
//this.Center = -this.Center; // Same as inverting, but we need to do it this way because of Pov-Ray
this.Invert = !this.Invert;
}
else
{
this.Invert = !this.Invert;
}
Debug.Assert(this.IsPointInside(interiorPoint));
return;
}
// Both planes?
if (IsPlane && sphere.IsPlane)
{
// XXX - not general, but I know the planes I'll be dealing with go through the origin.
//if( !sphere.Offset.IsOrigin )
// throw new System.NotImplementedException();
/*Vector3D p1 = this.Normal.Cross( sphere.Normal );
* if( !p1.Normalize() )
* {
* this.Center *= -1;
* return;
* }
*
* Vector3D p2 = p1.Cross( this.Normal );
* p2.Normalize();
* p1 = sphere.ReflectPoint( p1 );
* p2 = sphere.ReflectPoint( p2 );
* Vector3D newNormal = p2.Cross( p1 );
* if( !newNormal.Normalize() )
* throw new System.Exception( "Reflection impl" );
* this.Center = newNormal;*/
// Reflect the normal relative to the plane (conjugate with sphere.Offset).
Vector3D newNormal = this.Normal + sphere.Offset;
newNormal = sphere.ReflectPoint(newNormal);
newNormal -= sphere.Offset;
newNormal.Normalize();
this.Center = newNormal;
// Calc the new offset (so far we have considered planes through origin).
this.Offset = sphere.ReflectPoint(this.Offset);
//Debug.Assert( Offset.IsOrigin ); // XXX - should handle more generality.
Debug.Assert(this.IsPointInside(interiorPoint));
return;
}
// We are a plane and reflecting in a sphere.
if (IsPlane)
{
// Think of 2D case here (circle and line)...
Vector3D projected = Euclidean3D.ProjectOntoPlane(this.Normal, this.Offset, sphere.Center);
Vector3D p = sphere.ReflectPoint(projected);
if (Infinity.IsInfinite(p))
{
// This can happen if we go through sphere.Center.
// This reflection does not change our orientation (does not invert us).
return;
}
Center = sphere.Center + (p - sphere.Center) / 2;
Radius = Center.Dist(sphere.Center);
// Did this invert us?
if (!this.IsPointInside(interiorPoint))
{
Invert = !Invert;
}
return;
}
// Is mirror a plane?
if (sphere.IsPlane)
{
Vector3D projected = Euclidean3D.ProjectOntoPlane(sphere.Normal, sphere.Offset, Center);
Vector3D diff = Center - projected;
Center -= 2 * diff;
// Radius remains unchanged.
// NOTE: This does not invert us.
Debug.Assert(this.IsPointInside(interiorPoint));
return;
}
//
// Now sphere reflecting in a sphere.
//
// Reflecting to a plane?
if (IsPointOn(sphere.Center))
{
// Concentric spheres?
if (Center == sphere.Center)
{
throw new System.Exception();
}
// Center
Vector3D center = Center - sphere.Center;
// Offset
Vector3D direction = center;
direction.Normalize();
Vector3D offset = direction * Radius * 2;
offset = sphere.ReflectPoint(offset);
// We are a line now.
Center = center;
//Offset = offset; // Not working?? Caused issues in old generation code for 435.
Radius = double.PositiveInfinity;
// Did this invert us?
if (!this.IsPointInside(interiorPoint))
{
this.Invert = !this.Invert;
}
Debug.Assert(this.IsPointInside(interiorPoint));
return;
}
// XXX - Could try to share code below with Circle class.
// NOTE: We can't just reflect the center.
// See http://mathworld.wolfram.com/Inversion.html
double a = Radius;
double k = sphere.Radius;
Vector3D v = Center - sphere.Center;
double s = k * k / (v.MagSquared() - a * a);
Center = sphere.Center + v * s;
Radius = Math.Abs(s) * a;
// Did this invert us?
if (!this.IsPointInside(interiorPoint))
{
Invert = !Invert;
}
}
/// <summary>
/// {@inheritDoc}
/// </summary>
/// <exception cref="ConvergenceException"> if the algorithm failed due to finite
/// precision. </exception>
protected internal override sealed double DoSolve()
{
// Get initial solution
double x0 = this.Min;
double x1 = this.Max;
double f0 = this.ComputeObjectiveValue(x0);
double f1 = this.ComputeObjectiveValue(x1);
// If one of the bounds is the exact root, return it. Since these are
// not under-approximations or over-approximations, we can return them
// regardless of the allowed solutions.
if (f0 == 0.0)
{
return(x0);
}
if (f1 == 0.0)
{
return(x1);
}
// Verify bracketing of initial solution.
this.VerifyBracketing(x0, x1);
// Get accuracies.
double ftol = this.FunctionValueAccuracy;
double atol = this.AbsoluteAccuracy;
double rtol = this.RelativeAccuracy;
// Keep track of inverted intervals, meaning that the left bound is
// larger than the right bound.
bool inverted = false;
// Keep finding better approximations.
while (true)
{
// Calculate the next approximation.
double x = x1 - ((f1 * (x1 - x0)) / (f1 - f0));
double fx = this.ComputeObjectiveValue(x);
// If the new approximation is the exact root, return it. Since
// this is not an under-approximation or an over-approximation,
// we can return it regardless of the allowed solutions.
if (fx == 0.0)
{
return(x);
}
// Update the bounds with the new approximation.
if (f1 * fx < 0)
{
// The value of x1 has switched to the other bound, thus inverting
// the interval.
x0 = x1;
f0 = f1;
inverted = !inverted;
}
else
{
switch (this.method)
{
case BaseSecantSolver.Method.ILLINOIS:
f0 *= 0.5;
break;
case BaseSecantSolver.Method.PEGASUS:
f0 *= f1 / (f1 + fx);
break;
case Method.REGULA_FALSI:
// Detect early that algorithm is stuck, instead of waiting
// for the maximum number of iterations to be exceeded.
if (x == x1)
{
throw new ConvergenceException();
}
break;
default:
// Should never happen.
throw new MathInternalError();
}
}
// Update from [x0, x1] to [x0, x].
x1 = x;
f1 = fx;
// If the function value of the last approximation is too small,
// given the function value accuracy, then we can't get closer to
// the root than we already are.
if (FastMath.Abs(f1) <= ftol)
{
switch (this.allowed)
{
case AllowedSolution.ANY_SIDE:
return(x1);
case AllowedSolution.LEFT_SIDE:
if (inverted)
{
return(x1);
}
break;
case AllowedSolution.RIGHT_SIDE:
if (!inverted)
{
return(x1);
}
break;
case AllowedSolution.BELOW_SIDE:
if (f1 <= 0)
{
return(x1);
}
break;
case AllowedSolution.ABOVE_SIDE:
if (f1 >= 0)
{
return(x1);
}
break;
default:
throw new MathInternalError();
}
}
// If the current interval is within the given accuracies, we
// are satisfied with the current approximation.
if (FastMath.Abs(x1 - x0) < FastMath.Max(rtol * FastMath.Abs(x1), atol))
{
switch (this.allowed)
{
case AllowedSolution.ANY_SIDE:
return(x1);
case AllowedSolution.LEFT_SIDE:
return(inverted ? x1 : x0);
case AllowedSolution.RIGHT_SIDE:
return(inverted ? x0 : x1);
case AllowedSolution.BELOW_SIDE:
return((f1 <= 0) ? x1 : x0);
case AllowedSolution.ABOVE_SIDE:
return((f1 >= 0) ? x1 : x0);
default:
throw new MathInternalError();
}
}
}
}
/// <summary>
/// Equals the specified v and e.
/// </summary>
/// <returns>The equals.</returns>
/// <param name="v">V.</param>
/// <param name="e">E.</param>
public bool Equals(DVector2 v, double e = 0.00005)
{
return((M.Abs(x - v.x) <= e) && (M.Abs(y - v.y) <= e));
}
/// <summary>
/// {@inheritDoc} </summary>
protected internal override double DoIntegrate()
{
// Compute first estimate with a single step.
double oldt = this.Stage(1);
int n = 2;
while (true)
{
// Improve integral with a larger number of steps.
double t = this.Stage(n);
// Estimate the error.
double delta = FastMath.Abs(t - oldt);
double limit = FastMath.Max(this.AbsoluteAccuracy, this.RelativeAccuracy * (FastMath.Abs(oldt) + FastMath.Abs(t)) * 0.5);
// check convergence
if (this.iterations.Count + 1 >= this.MinimalIterationCount && delta <= limit)
{
return(t);
}
// Prepare next iteration.
double ratio = FastMath.Min(4, FastMath.Pow(delta / limit, 0.5 / this.numberOfPoints));
n = FastMath.Max((int)(ratio * n), n + 1);
oldt = t;
this.iterations.IncrementCount();
}
}
/// <summary>
/// {@inheritDoc}
/// </summary>
protected internal override double DoSolve()
{
double min = this.Min;
double max = this.Max;
// [x1, x2] is the bracketing interval in each iteration
// x3 is the midpoint of [x1, x2]
// x is the new root approximation and an endpoint of the new interval
double x1 = min;
double y1 = this.ComputeObjectiveValue(x1);
double x2 = max;
double y2 = this.ComputeObjectiveValue(x2);
// check for zeros before verifying bracketing
if (y1 == 0)
{
return(min);
}
if (y2 == 0)
{
return(max);
}
this.VerifyBracketing(min, max);
double absoluteAccuracy = this.AbsoluteAccuracy;
double functionValueAccuracy = this.FunctionValueAccuracy;
double relativeAccuracy = this.RelativeAccuracy;
double oldx = double.PositiveInfinity;
while (true)
{
// calculate the new root approximation
double x3 = 0.5 * (x1 + x2);
double y3 = this.ComputeObjectiveValue(x3);
if (FastMath.Abs(y3) <= functionValueAccuracy)
{
return(x3);
}
double delta = 1 - (y1 * y2) / (y3 * y3); // delta > 1 due to bracketing
double correction = (MyUtils.Signum(y2) * MyUtils.Signum(y3)) * (x3 - x1) / FastMath.Sqrt(delta);
double x = x3 - correction; // correction != 0
double y = this.ComputeObjectiveValue(x);
// check for convergence
double tolerance = FastMath.Max(relativeAccuracy * FastMath.Abs(x), absoluteAccuracy);
if (FastMath.Abs(x - oldx) <= tolerance)
{
return(x);
}
if (FastMath.Abs(y) <= functionValueAccuracy)
{
return(x);
}
// prepare the new interval for next iteration
// Ridders' method guarantees x1 < x < x2
if (correction > 0.0) // x1 < x < x3
{
if (MyUtils.Signum(y1) + MyUtils.Signum(y) == 0.0)
{
x2 = x;
y2 = y;
}
else
{
x1 = x;
x2 = x3;
y1 = y;
y2 = y3;
}
} // x3 < x < x2
else
{
if (MyUtils.Signum(y2) + MyUtils.Signum(y) == 0.0)
{
x1 = x;
y1 = y;
}
else
{
x1 = x3;
x2 = x;
y1 = y3;
y2 = y;
}
}
oldx = x;
}
}
public void Initialize()
{
// Heightmap Map to water level creation ( only required for water )
//---------------------------------------------------------------------
if (underwaterHeightView != null)
{
underwaterHeightView.Dispose();
}
if (normalView != null)
{
normalView.Dispose();
}
Texture2DDescription descHM = new Texture2DDescription()
{
ArraySize = 1,
BindFlags = BindFlags.None,
CpuAccessFlags = CpuAccessFlags.Write,
Format = Format.R8_UNorm,
Width = info.width,
Height = info.height,
MipLevels = 1,
OptionFlags = ResourceOptionFlags.None,
SampleDescription = new SharpDX.DXGI.SampleDescription(1, 0),
Usage = ResourceUsage.Staging,
};
Texture2D MappableTexture = new Texture2D(Display.device, descHM);
Surface surf = MappableTexture.QueryInterface <Surface>();
DataStream stream;
DataRectangle rect = surf.Map(SharpDX.DXGI.MapFlags.Write, out stream);
stream.Position = 0;
float delta = 1;
//float level_clean = saturate((level-0.95f)/(1-0.95f));
for (int y = 0; y < info.height; y++)
{
for (int x = 0; x < info.width; x++)
{
float val = ((info.altitudeData[x, y] - (info.waterLevel - delta)) / delta);
stream.WriteByte((byte)GameUtils.Clamp(val * 255, 0, 255));
}
}
surf.Unmap();
var desc = new Texture2DDescription();
desc.Width = info.width;
desc.Height = info.height;
desc.MipLevels = 1;
desc.ArraySize = 1;
desc.Format = SharpDX.DXGI.Format.R8_UNorm;
desc.Usage = ResourceUsage.Default;
desc.BindFlags = BindFlags.ShaderResource;
desc.CpuAccessFlags = CpuAccessFlags.None;
desc.SampleDescription = new SampleDescription(1, 0);
Texture2D newText = new Texture2D(Display.device, desc);
Display.context.CopyResource(MappableTexture, newText);
MappableTexture.Dispose();
underwaterHeightView = new ShaderResourceView(Display.device, newText);
// Normal Map creation
//---------------------------------------------------------------------
descHM = new Texture2DDescription()
{
ArraySize = 1,
BindFlags = BindFlags.None,
CpuAccessFlags = CpuAccessFlags.Write,
Format = Format.R8G8B8A8_UNorm,
Width = info.width,
Height = info.height,
MipLevels = 1,
OptionFlags = ResourceOptionFlags.None,
SampleDescription = new SharpDX.DXGI.SampleDescription(1, 0),
Usage = ResourceUsage.Staging,
};
MappableTexture = new Texture2D(Display.device, descHM);
surf = MappableTexture.QueryInterface <Surface>();
DataStream streamN;
rect = surf.Map(SharpDX.DXGI.MapFlags.Write, out streamN);
streamN.Position = 0;
for (int y = 0; y < info.height; y++)
{
for (int x = 0; x < info.width; x++)
{
Vector3 data = info.normalData[x, y];
data.X = Math.Abs(data.X);
data.Y = Math.Max(data.Y, 0);
data.Z = Math.Abs(data.Z);
data.X = GameUtils.Clamp(data.X * 255, 0, 255);
data.Y = GameUtils.Clamp(data.Y * 255, 0, 255);
data.Z = GameUtils.Clamp(data.Z * 255, 0, 255);
streamN.WriteByte((byte)(data.X));
streamN.WriteByte((byte)(data.Y));
streamN.WriteByte((byte)(data.Z));
streamN.WriteByte(0xFF);
}
}
surf.Unmap();
desc = new Texture2DDescription();
desc.Width = info.width;
desc.Height = info.height;
desc.MipLevels = 1;
desc.ArraySize = 1;
desc.Format = SharpDX.DXGI.Format.R8G8B8A8_UNorm;
desc.Usage = ResourceUsage.Default;
desc.BindFlags = BindFlags.ShaderResource;
desc.CpuAccessFlags = CpuAccessFlags.None;
desc.SampleDescription = new SampleDescription(1, 0);
newText = new Texture2D(Display.device, desc);
Display.context.CopyResource(MappableTexture, newText);
MappableTexture.Dispose();
normalView = new ShaderResourceView(Display.device, newText);
}
protected override void OnDraw(Canvas canvas)
{
base.OnDraw(canvas);
if (mViewPager == null)
{
return;
}
int count = mViewPager.Adapter.Count;
if (count == 0)
{
return;
}
if (mCurrentPage >= count)
{
SetCurrentItem(count - 1);
return;
}
int longSize;
int longPaddingBefore;
int longPaddingAfter;
int shortPaddingBefore;
if (mOrientation == HORIZONTAL)
{
longSize = Width;
longPaddingBefore = PaddingLeft;
longPaddingAfter = PaddingRight;
shortPaddingBefore = PaddingTop;
}
else
{
longSize = Height;
longPaddingBefore = PaddingTop;
longPaddingAfter = PaddingBottom;
shortPaddingBefore = PaddingLeft;
}
float threeRadius = mRadius * 3;
float shortOffset = shortPaddingBefore + mRadius;
float longOffset = longPaddingBefore + mRadius;
if (mCentered)
{
longOffset += ((longSize - longPaddingBefore - longPaddingAfter) / 2.0f) - ((count * threeRadius) / 2.0f);
}
float dX;
float dY;
float pageFillRadius = mRadius;
if (mPaintStroke.StrokeWidth > 0)
{
pageFillRadius -= mPaintStroke.StrokeWidth / 2.0f;
}
//Draw stroked circles
for (int iLoop = 0; iLoop < count; iLoop++)
{
float drawLong = longOffset + (iLoop * threeRadius);
if (mOrientation == HORIZONTAL)
{
dX = drawLong;
dY = shortOffset;
}
else
{
dX = shortOffset;
dY = drawLong;
}
// Only paint fill if not completely transparent
if (mPaintPageFill.Alpha > 0)
{
canvas.DrawCircle(dX, dY, pageFillRadius, mPaintPageFill);
}
// Only paint stroke if a stroke width was non-zero
if (Math.Abs(pageFillRadius - mRadius) > double.Epsilon)
{
canvas.DrawCircle(dX, dY, mRadius, mPaintStroke);
}
}
//Draw the filled circle according to the current scroll
float cx = (mSnap ? mSnapPage : mCurrentPage) * threeRadius;
if (!mSnap && (mPageSize != 0))
{
cx += (mCurrentOffset * 1.0f / mPageSize) * threeRadius;
}
if (mOrientation == HORIZONTAL)
{
dX = longOffset + cx;
dY = shortOffset;
}
else
{
dX = shortOffset;
dY = longOffset + cx;
}
canvas.DrawCircle(dX, dY, mRadius, mPaintFill);
}
public void Update(float dt)
{
// Reset deltas
MovementNormalizedDelta = new Float4(0.0f);
MovementRawDelta = new Float4(0.0f);
MovementDirectDelta = new Float4(0.0f);
RotationDelta = new Float4(0.0f);
RotationQuaternionDelta = Quaternion.Default;
ScaleDelta = new Float4(0.0f);
FovDelta = 0.0f;
MovementNormalizedLength = 0.0f;
if (ActiveMapping != null)
{
foreach (var input in ActiveMapping.KeyInputs)
{
if (CheckKeyInput(input.Key, input.Type))
{
// Skip single keys if there is a combo key input active
bool skipKey = false;
foreach (var comboKeyInput in ActiveMapping.ComboKeyInputs.Where(x => x.ConsumeInput && (x.Key1 == input.Key || x.Key2 == input.Key)))
{
var key = comboKeyInput.Key1 == input.Key ? comboKeyInput.Key2 : comboKeyInput.Key1;
var type = comboKeyInput.Key1 == input.Key ? comboKeyInput.Type2 : comboKeyInput.Type1;
if (CheckKeyInput(key, type))
{
skipKey = true;
break;
}
}
if (skipKey)
{
continue;
}
input.Action.Execute();
}
}
foreach (var input in ActiveMapping.ComboKeyInputs)
{
if (CheckKeyInput(input.Key1, input.Type1) && CheckKeyInput(input.Key2, input.Type2))
{
input.Action.Execute();
}
}
foreach (var input in ActiveMapping.MouseInputs)
{
if (CheckMouseInput(input.Key, input.Type))
{
input.Action.Execute();
}
}
foreach (var input in ActiveMapping.ScrollInputs)
{
if (CheckScrollInput(input.Key))
{
input.Action.Execute();
}
}
if (CaptureMouse)
{
Float2 mouseDiff = Application.GetCursorDiff(true);
foreach (var input in ActiveMapping.MouseAxisInputs)
{
float length = input.Axis.Dot(mouseDiff);
if (Math.Abs(length) > Constants.Epsilon)
{
if (input.Action is InputMovementAction movementAction)
{
// Copy of the InputMovementAction.Execute function but multiplied with length for mouse input
var vector = movementAction.GetVector() * length;
MovementRawDelta += vector;
if (movementAction.Normalize)
{
MovementNormalizedLength = Math.Max(vector.Length(), MovementNormalizedLength);
}
else
{
MovementDirectDelta += vector;
}
}
else if (input.Action is InputRotationAction rotationAction)
{
RotationDelta += rotationAction.GetRotation() * length;
// TODO: move dt to UpdateEntities
RotationQuaternionDelta *= rotationAction.GetQuaternion(length * dt);
}
else if (input.Action is InputScalingAction scaleAction)
{
ScaleDelta += scaleAction.GetVector() * length;
}
else
{
input.Action.Execute();
}
}
}
}
}
MovementNormalizedDelta += MovementRawDelta.Normalize() * MovementNormalizedLength;
UpdateEntities(dt);
}
/// <summary>
/// Determines whether the specified value is equal to the current <see cref="Vector2"/>.
/// </summary>
/// <param name="value"></param>
/// <returns></returns>
public bool Equals(float value)
{
return(Maths.Abs(X - value) < float.Epsilon && Maths.Abs(Y - value) < float.Epsilon);
}
void ReadPng()
{
for (int i = 0; i < PNGID.Length; i++)
{
Assert(PNGID[i] == Inp.ReadByte());
}
byte[] buffer = new byte[1024];
while (true)
{
int len = GetInt(Inp);
uint chunktype = GetUInt(Inp);
Assert(len >= 0);
if (chunktype == IHDR) // header
{
Width = GetInt(Inp);
Height = GetInt(Inp);
BitDepth = Inp.ReadByte();
ColorType = Inp.ReadByte();
CompressionMethod = Inp.ReadByte();
FilterMethod = Inp.ReadByte();
InterlaceMethod = Inp.ReadByte();
}
else if (chunktype == IDAT) // the image data
{
Util.Copy(Inp, Idat, len, buffer);
}
else if (chunktype == tRNS) // transparency information
{
switch (ColorType)
{
case 0:
if (len >= 2)
{
len -= 2;
int gray = GetWord(Inp);
if (BitDepth == 16)
{
TransRedGray = gray;
}
else
{
Additional.Put(DictName.Mask, "[ " + gray + " " + gray + " ]");
}
}
break;
case 2:
if (len >= 6)
{
len -= 6;
int red = GetWord(Inp);
int green = GetWord(Inp);
int blue = GetWord(Inp);
if (BitDepth == 16)
{
TransRedGray = red;
TransGreen = green;
TransBlue = blue;
}
else
{
Additional.Put(DictName.Mask, "[ " + red + " " + red + " " + green + " " + green + " " + blue + " " + blue + " ]");
}
}
break;
case 3:
if (len > 0)
{
Trans = new byte[len];
for (int k = 0; k < len; ++k)
{
Trans[k] = ( byte )Inp.ReadByte();
}
len = 0;
}
break;
}
Util.Skip(Inp, len);
}
else if (chunktype == PLTE) // contains the palette; list of colors.
{
if (ColorType == 3)
{
DictArray colorspace = new DictArray();
colorspace.Add(DictName.Indexed);
colorspace.Add(GetColorspace());
colorspace.Add(len / 3 - 1);
ColorTable = new byte[len];
int ix = 0; while ((len--) > 0)
{
ColorTable[ix++] = ( byte )Inp.ReadByte();
}
colorspace.Add(new PdfByteStr(ColorTable));
Additional.Put(DictName.ColorSpace, colorspace);
}
else
{
Util.Skip(Inp, len);
}
}
else if (chunktype == pHYs) // Currently nothing is done with this info.
{
int dx = GetInt(Inp);
int dy = GetInt(Inp);
int unit = Inp.ReadByte();
if (unit == 1)
{
DpiX = ( int )(( float )dx * 0.0254f + 0.5f);
DpiY = ( int )(( float )dy * 0.0254f + 0.5f);
}
else
{
if (dy != 0)
{
XYRatio = ( float )dx / ( float )dy;
}
}
}
else if (chunktype == cHRM) // gives the chromaticity coordinates of the display primaries and white point.
{
xW = ( float )GetInt(Inp) / 100000f;
yW = ( float )GetInt(Inp) / 100000f;
xR = ( float )GetInt(Inp) / 100000f;
yR = ( float )GetInt(Inp) / 100000f;
xG = ( float )GetInt(Inp) / 100000f;
yG = ( float )GetInt(Inp) / 100000f;
xB = ( float )GetInt(Inp) / 100000f;
yB = ( float )GetInt(Inp) / 100000f;
HasCHRM = !(Math.Abs(xW) < 0.0001f || Math.Abs(yW) < 0.0001f || Math.Abs(xR) < 0.0001f || Math.Abs(yR) < 0.0001f ||
Math.Abs(xG) < 0.0001f || Math.Abs(yG) < 0.0001f || Math.Abs(xB) < 0.0001f || Math.Abs(yB) < 0.0001f);
}
else if (chunktype == sRGB) // indicates that the standard sRGB color space is used.
{
int ri = Inp.ReadByte();
Intent = Intents[ri];
Gamma = 2.2f;
xW = 0.3127f; yW = 0.329f; xR = 0.64f; yR = 0.33f;
xG = 0.3f; yG = 0.6f; xB = 0.15f; yB = 0.06f;
HasCHRM = true;
}
else if (chunktype == gAMA)
{
int gm = GetInt(Inp);
if (gm != 0)
{
Gamma = 100000f / ( float )gm;
if (!HasCHRM)
{
xW = 0.3127f; yW = 0.329f; xR = 0.64f; yR = 0.33f;
xG = 0.3f; yG = 0.6f; xB = 0.15f; yB = 0.06f;
HasCHRM = true;
}
}
}
else if (chunktype == iCCP)
{
// Console.WriteLine( "iCCP chunk found" );
do
{
len -= 1;
} while (Inp.ReadByte() != 0);
Inp.ReadByte(); len -= 1;
byte[] icc = new byte[len];
Util.ReadN(Inp, icc, 0, len);
icc = Util.Inflate(icc);
ICCP = ICC_Profile.GetInstance(icc);
}
else if (chunktype == IEND)
{
break;
}
else
{
Util.Skip(Inp, len);
}
Util.Skip(Inp, 4);
}
}
/// <summary>
/// Determines whether the specified <see cref="Vector2"/> values are equal.
/// </summary>
/// <param name="position1"></param>
/// <param name="position2"></param>
/// <returns></returns>
public static bool Equals(Vector2 position1, Vector2 position2)
{
return(Maths.Abs(position1.X - position2.X) < float.Epsilon && Maths.Abs(position1.Y - position2.Y) < float.Epsilon);
}
/// <summary>
/// Returns the angle in degrees between from and to.
/// </summary>
/// <param name="from"></param>
/// <param name="to"></param>
/// <returns></returns>
public static float Angle(Vector2 from, Vector2 to)
{
return(Maths.Abs(SignedAngle(from, to)));
}
public bool Collide(Box2 b)
{
return(SysMath.Abs(Min.X - b.Min.X) * 2 < Width + b.Width &&
SysMath.Abs(Min.Y - b.Min.Y) * 2 < Height + b.Height);
}
private static float abs(float x)
{
return((float)Math.Abs(x));
}
/// <summary>
/// Set all vector components to their absolute value.
/// </summary>
public static Vector3Int AbsAll(this Vector3Int v)
{
return(new Vector3Int(Math.Abs(v.x), Math.Abs(v.y), Math.Abs(v.z)));
}
/// <summary>
/// Search for a zero inside the provided interval.
/// This implementation is based on the algorithm described at page 58 of
/// the book
/// <blockquote>
/// <b>Algorithms for Minimization Without Derivatives</b>,
/// <it>Richard P. Brent</it>,
/// Dover 0-486-41998-3
/// </blockquote>
/// </summary>
/// <param name="lo"> Lower bound of the search interval. </param>
/// <param name="hi"> Higher bound of the search interval. </param>
/// <param name="fLo"> Function value at the lower bound of the search interval. </param>
/// <param name="fHi"> Function value at the higher bound of the search interval. </param>
/// <returns> the value where the function is zero. </returns>
private double Brent(double lo, double hi, double fLo, double fHi)
{
double a = lo;
double fa = fLo;
double b = hi;
double fb = fHi;
double c = a;
double fc = fa;
double d = b - a;
double e = d;
//JAVA TO C# CONVERTER WARNING: The original Java variable was marked 'final':
//ORIGINAL LINE: final double t = getAbsoluteAccuracy();
double t = GetAbsoluteAccuracy();
//JAVA TO C# CONVERTER WARNING: The original Java variable was marked 'final':
//ORIGINAL LINE: final double eps = getRelativeAccuracy();
double eps = GetRelativeAccuracy();
while (true)
{
if (FastMath.Abs(fc) < FastMath.Abs(fb))
{
a = b;
b = c;
c = a;
fa = fb;
fb = fc;
fc = fa;
}
//JAVA TO C# CONVERTER WARNING: The original Java variable was marked 'final':
//ORIGINAL LINE: final double tol = 2 * eps * org.apache.commons.math3.util.FastMath.abs(b) + t;
double tol = 2 * eps * FastMath.Abs(b) + t;
//JAVA TO C# CONVERTER WARNING: The original Java variable was marked 'final':
//ORIGINAL LINE: final double m = 0.5 * (c - b);
double m = 0.5 * (c - b);
if (FastMath.Abs(m) <= tol || Precision.Equals(fb, 0))
{
return(b);
}
if (FastMath.Abs(e) < tol || FastMath.Abs(fa) <= FastMath.Abs(fb))
{
// Force bisection.
d = m;
e = d;
}
else
{
double s = fb / fa;
double p;
double q;
// The equality test (a == c) is intentional,
// it is part of the original Brent's method and
// it should NOT be replaced by proximity test.
if (a == c)
{
// Linear interpolation.
p = 2 * m * s;
q = 1 - s;
}
else
{
// Inverse quadratic interpolation.
q = fa / fc;
//JAVA TO C# CONVERTER WARNING: The original Java variable was marked 'final':
//ORIGINAL LINE: final double r = fb / fc;
double r = fb / fc;
p = s * (2 * m * q * (q - r) - (b - a) * (r - 1));
q = (q - 1) * (r - 1) * (s - 1);
}
if (p > 0)
{
q = -q;
}
else
{
p = -p;
}
s = e;
e = d;
if (p >= 1.5 * m * q - FastMath.Abs(tol * q) || p >= FastMath.Abs(0.5 * s * q))
{
// Inverse quadratic interpolation gives a value
// in the wrong direction, or progress is slow.
// Fall back to bisection.
d = m;
e = d;
}
else
{
d = p / q;
}
}
a = b;
fa = fb;
if (FastMath.Abs(d) > tol)
{
b += d;
}
else if (m > 0)
{
b += tol;
}
else
{
b -= tol;
}
fb = ComputeObjectiveValue(b);
if ((fb > 0 && fc > 0) || (fb <= 0 && fc <= 0))
{
c = a;
fc = fa;
d = b - a;
e = d;
}
}
}
/// <summary>
/// Set the vector's y component to its absolute value.
/// </summary>
public static Vector3 AbsY(this Vector3 v)
{
return(new Vector3(v.x, Math.Abs(v.y), v.z));
}
/// <summary>
/// Returns the angle between a and b.
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <returns></returns>
public static float AngleBetween(Quaternion a, Quaternion b)
{
var dot = Dot(a, b);
return((float)((Maths.Acos(Maths.Min(Maths.Abs(dot), 1.0f)) * 2.0 * (180.0f / Mathf.PI))));
}
/// <summary>
/// Set the vector's x component to its absolute value.
/// </summary>
public static Vector3Int AbsX(this Vector3Int v)
{
return(new Vector3Int(Math.Abs(v.x), v.y, v.z));
}
public static float pitchPairDistance(int pitch1, int pitch2)
{
return(Math.Abs(KeyPositions[pitch1] - KeyPositions[pitch2]));
}
/// <summary>
/// Set the vector's z component to its absolute value.
/// </summary>
public static Vector3Int AbsZ(this Vector3Int v)
{
return(new Vector3Int(v.x, v.y, Math.Abs(v.z)));
}
private static int Math_Abs(ILuaState lua)
{
lua.PushNumber(Math.Abs(lua.L_CheckNumber(1)));
return(1);
}
//---------------------------------------------------------------------------
#region Private Member Data
//---------------------------------------------------------------------------
//---------------------------------------------------------------------------
#endregion
//---------------------------------------------------------------------------
#region Accessors and Mutators
//---------------------------------------------------------------------------
//---------------------------------------------------------------------------
#endregion
//---------------------------------------------------------------------------
#region Public Member Functions
//---------------------------------------------------------------------------
public static bool AreVectorsEqual(Vector3 left, Vector3 right)
{
Vector3 diffVector = right - left;
return(Math.Abs(diffVector.x) <= 0.5f && Math.Abs(diffVector.y) <= 0.5f && Math.Abs(diffVector.z) <= 0.5f);
}
public void Shift(Vector delta)
{
if (delta.X == 0 && delta.Y == 0 && AbsoluteCenterCoordinates != null)
{
return;
}
CenterCoordinates = CenterCoordinates.Shift(delta.X, delta.Y, Rotation, ArcSecWidth, ArcSecHeight);
AbsoluteCenterCoordinates = new Coordinates(CenterCoordinates.RADegrees, Math.Abs(CenterCoordinates.Dec), Epoch.J2000, Coordinates.RAType.Degrees);
TopCenter = AbsoluteCenterCoordinates.Shift(0, -OriginalVFoV / 2, 0);
TopLeft = AbsoluteCenterCoordinates.Shift(-OriginalHFoV / 2, (-OriginalVFoV / 2), 0);
BottomLeft = AbsoluteCenterCoordinates.Shift(-OriginalHFoV / 2, (OriginalVFoV / 2), 0);
BottomCenter = AbsoluteCenterCoordinates.Shift(0, OriginalVFoV / 2, 0);
VFoVDegTop = Math.Abs(Math.Max(TopCenter.Dec, TopLeft.Dec) - AbsoluteCenterCoordinates.Dec);
VFoVDegBottom = Math.Abs(AbsoluteCenterCoordinates.Dec - Math.Min(BottomLeft.Dec, BottomCenter.Dec));
HFoVDeg = TopLeft.RADegrees > TopCenter.RADegrees
? TopLeft.RADegrees - TopCenter.RADegrees
: TopLeft.RADegrees - TopCenter.RADegrees + 360;
HFoVDeg *= 2;
// if we're below 0 we need to flip all calculated decs
if (CenterCoordinates.Dec < 0)
{
BottomLeft.Dec *= -1;
TopLeft.Dec *= -1;
TopCenter.Dec *= -1;
BottomCenter.Dec *= -1;
AboveZero = false;
}
else
{
AboveZero = true;
}
// if the top center ra is different than the center coordinate ra then the top center point is above 90deg
if (Math.Abs(TopCenter.RADegrees - CenterCoordinates.RADegrees) > 0.0001)
{
// horizontal fov becomes 360 here and vertical fov the difference between center and 90deg
HFoVDeg = 360;
VFoVDegTop = 90 - AbsoluteCenterCoordinates.Dec;
IsAbove90 = true;
}
else
{
IsAbove90 = false;
}
AbsCalcTopDec = AbsoluteCenterCoordinates.Dec + VFoVDegTop;
AbsCalcBottomDec = AbsoluteCenterCoordinates.Dec - VFoVDegBottom;
if (AboveZero)
{
CalcTopDec = CenterCoordinates.Dec + VFoVDegTop;
CalcBottomDec = CenterCoordinates.Dec - VFoVDegBottom;
}
else
{
CalcTopDec = CenterCoordinates.Dec - VFoVDegTop;
CalcBottomDec = CenterCoordinates.Dec + VFoVDegBottom;
}
CalcRAMax = TopLeft.RADegrees;
CalcRAMin = CalcRAMax - HFoVDeg;
if (CalcRAMin < 0)
{
CalcRAMin += 360;
}
}
/// <summary>
/// {@inheritDoc} </summary>
protected internal override sealed double DoSolve()
{
// Get initial solution
double x0 = this.Min;
double x1 = this.Max;
double f0 = this.ComputeObjectiveValue(x0);
double f1 = this.ComputeObjectiveValue(x1);
// If one of the bounds is the exact root, return it. Since these are
// not under-approximations or over-approximations, we can return them
// regardless of the allowed solutions.
if (f0 == 0.0)
{
return(x0);
}
if (f1 == 0.0)
{
return(x1);
}
// Verify bracketing of initial solution.
this.VerifyBracketing(x0, x1);
// Get accuracies.
double ftol = this.FunctionValueAccuracy;
double atol = this.AbsoluteAccuracy;
double rtol = this.RelativeAccuracy;
// Keep finding better approximations.
while (true)
{
// Calculate the next approximation.
double x = x1 - ((f1 * (x1 - x0)) / (f1 - f0));
double fx = this.ComputeObjectiveValue(x);
// If the new approximation is the exact root, return it. Since
// this is not an under-approximation or an over-approximation,
// we can return it regardless of the allowed solutions.
if (fx == 0.0)
{
return(x);
}
// Update the bounds with the new approximation.
x0 = x1;
f0 = f1;
x1 = x;
f1 = fx;
// If the function value of the last approximation is too small,
// given the function value accuracy, then we can't get closer to
// the root than we already are.
if (FastMath.Abs(f1) <= ftol)
{
return(x1);
}
// If the current interval is within the given accuracies, we
// are satisfied with the current approximation.
if (FastMath.Abs(x1 - x0) < FastMath.Max(rtol * FastMath.Abs(x1), atol))
{
return(x1);
}
}
}
public static void Main()
{
//Post methods
//THIS SECTION CREATES / INITIALIZES THE SERIAL LOGGER
Debug.Print("Flight computer posted successfully. Beginning INIT.");
//Initialize sensors, etc.
Debug.Print("Starting stopwatch");
Clock.Instance.Start();
//Thread.Sleep(5000);
Debug.Print("Flight computer INIT Complete. Continuing with boot.");
//THIS SECTION INITIALIZES AND STARTS THE MEMORY MONITOR
Debug.Print("Starting memory monitor...");
MemoryMonitor.Instance.Start();
var _motors = new MotorController();
var testPing = new PingUpdater(Pins.GPIO_PIN_A0);
testPing.Start();
var testIR = new IRDistanceUpdater(AnalogChannels.ANALOG_PIN_A1, 25, 100);
testIR.Start();
// Start sensor actions here.
Debug.Print("Flight computer boot successful.");
while (true)
{
Debug.Print("IR: " + RobotState.IRDistance);
Debug.Print("\nPing: " + RobotState.PingDistance);
Thread.Sleep(1000);
var oldSenV = RobotState.LastIRDistance;
var currentSenV = RobotState.IRDistance;
GreenLED.Write(RobotState.CheckReady());
BlueLED.Write(currentSenV >= 1000);
YellowLED.Write(currentSenV >= 2000);
if (currentSenV >= 1000)
{
BlueLED.Write(true);
}
if (Math.Abs(oldSenV - currentSenV) > .01)
{
Debug.Print("SensorValue: " + currentSenV);
RedLED.Write(false);
YellowLED.Write(false);
BlueLED.Write(false);
if (currentSenV >= 1000)
{
BlueLED.Write(true);
}
if (currentSenV >= 2000)
{
YellowLED.Write(true);
}
if (!(currentSenV >= 3000))
{
continue;
}
RedLED.Write(true);
_motors.Halt();
_motors.Backward(255);
Thread.Sleep(100);
if (currentSenV >= 2000)
{
//do nothing
Debug.Print("Too close...");
_motors.Halt();
_motors.Right(255);
}
}
}
}
public static SvdResult ComputeSvd(double[,] matrix)
{
double[,] U, V;
double[] s;
// Derived from LINPACK code.
// Initialize.
double[,] A = matrix.Copy();
int m = matrix.RowCount();
int n = matrix.ColumnCount();
int nu = M.Min(m, n);
s = new double[M.Min(m + 1, n)];
U = new double[m, nu];
V = new double[n, n];
var e = new double[n];
var work = new double[m];
bool wantu = true;
bool wantv = true;
// Reduce A to bidiagonal form, storing the diagonal elements
// in s and the super-diagonal elements in e.
int nct = M.Min(m - 1, n);
int nrt = M.Max(0, M.Min(n - 2, m));
for (int k = 0; k < M.Max(nct, nrt); k++)
{
if (k < nct)
{
// Compute the transformation for the k-th column and
// place the k-th diagonal in s[k].
// Compute 2-norm of k-th column without under/overflow.
s[k] = 0;
for (int i = k; i < m; i++)
{
s[k] = Functions.Hypotenuse(s[k], A[i, k]);
}
if (s[k] != 0.0)
{
if (A[k, k] < 0.0)
{
s[k] = -s[k];
}
for (int i = k; i < m; i++)
{
A[i, k] /= s[k];
}
A[k, k] += 1.0;
}
s[k] = -s[k];
}
for (int j = k + 1; j < n; j++)
{
if ((k < nct) & (s[k] != 0.0))
{
// Apply the transformation.
double t = 0;
for (int i = k; i < m; i++)
{
t += A[i, k] * A[i, j];
}
t = (-t) / A[k, k];
for (int i = k; i < m; i++)
{
A[i, j] += t * A[i, k];
}
}
// Place the k-th row of A into e for the
// subsequent calculation of the row transformation.
e[j] = A[k, j];
}
if (wantu & (k < nct))
{
// Place the transformation in U for subsequent back
// multiplication.
for (int i = k; i < m; i++)
{
U[i, k] = A[i, k];
}
}
if (k < nrt)
{
// Compute the k-th row transformation and place the
// k-th super-diagonal in e[k].
// Compute 2-norm without under/overflow.
e[k] = 0;
for (int i = k + 1; i < n; i++)
{
e[k] = Functions.Hypotenuse(e[k], e[i]);
}
if (e[k] != 0.0)
{
if (e[k + 1] < 0.0)
{
e[k] = -e[k];
}
for (int i = k + 1; i < n; i++)
{
e[i] /= e[k];
}
e[k + 1] += 1.0;
}
e[k] = -e[k];
if ((k + 1 < m) & (e[k] != 0.0))
{
// Apply the transformation.
for (int i = k + 1; i < m; i++)
{
work[i] = 0.0;
}
for (int j = k + 1; j < n; j++)
{
for (int i = k + 1; i < m; i++)
{
work[i] += e[j] * A[i, j];
}
}
for (int j = k + 1; j < n; j++)
{
double t = (-e[j]) / e[k + 1];
for (int i = k + 1; i < m; i++)
{
A[i, j] += t * work[i];
}
}
}
if (wantv)
{
// Place the transformation in V for subsequent
// back multiplication.
for (int i = k + 1; i < n; i++)
{
V[i, k] = e[i];
}
}
}
}
// Set up the final bidiagonal matrix or order p.
int p = M.Min(n, m + 1);
if (nct < n)
{
s[nct] = A[nct, nct];
}
if (m < p)
{
s[p - 1] = 0.0;
}
if (nrt + 1 < p)
{
e[nrt] = A[nrt, p - 1];
}
e[p - 1] = 0.0;
// If required, generate U.
if (wantu)
{
for (int j = nct; j < nu; j++)
{
for (int i = 0; i < m; i++)
{
U[i, j] = 0.0;
}
U[j, j] = 1.0;
}
for (int k = nct - 1; k >= 0; k--)
{
if (s[k] != 0.0)
{
for (int j = k + 1; j < nu; j++)
{
double t = 0;
for (int i = k; i < m; i++)
{
t += U[i, k] * U[i, j];
}
t = (-t) / U[k, k];
for (int i = k; i < m; i++)
{
U[i, j] += t * U[i, k];
}
}
for (int i = k; i < m; i++)
{
U[i, k] = -U[i, k];
}
U[k, k] = 1.0 + U[k, k];
for (int i = 0; i < k - 1; i++)
{
U[i, k] = 0.0;
}
}
else
{
for (int i = 0; i < m; i++)
{
U[i, k] = 0.0;
}
U[k, k] = 1.0;
}
}
}
// If required, generate V.
if (wantv)
{
for (int k = n - 1; k >= 0; k--)
{
if ((k < nrt) & (e[k] != 0.0))
{
for (int j = k + 1; j < nu; j++)
{
double t = 0;
for (int i = k + 1; i < n; i++)
{
t += V[i, k] * V[i, j];
}
t = (-t) / V[k + 1, k];
for (int i = k + 1; i < n; i++)
{
V[i, j] += t * V[i, k];
}
}
}
for (int i = 0; i < n; i++)
{
V[i, k] = 0.0;
}
V[k, k] = 1.0;
}
}
// Main iteration loop for the singular values.
int pp = p - 1;
int iter = 0;
double eps = M.Pow(2.0, -52.0);
while (p > 0)
{
int k, kase;
// Here is where a test for too many iterations would go.
// This section of the program inspects for
// negligible elements in the s and e arrays. On
// completion the variables kase and k are set as follows.
// kase = 1 if s(p) and e[k-1] are negligible and k<p
// kase = 2 if s(k) is negligible and k<p
// kase = 3 if e[k-1] is negligible, k<p, and
// s(k), ..., s(p) are not negligible (qr step).
// kase = 4 if e(p-1) is negligible (convergence).
for (k = p - 2; k >= -1; k--)
{
if (k == -1)
{
break;
}
if (M.Abs(e[k]) <= eps * (M.Abs(s[k]) + M.Abs(s[k + 1])))
{
e[k] = 0.0;
break;
}
}
if (k == p - 2)
{
kase = 4;
}
else
{
int ks;
for (ks = p - 1; ks >= k; ks--)
{
if (ks == k)
{
break;
}
double t = (ks != p ? M.Abs(e[ks]) : 0.0) + (ks != k + 1 ? M.Abs(e[ks - 1]) : 0.0);
if (M.Abs(s[ks]) <= eps * t)
{
s[ks] = 0.0;
break;
}
}
if (ks == k)
{
kase = 3;
}
else if (ks == p - 1)
{
kase = 1;
}
else
{
kase = 2;
k = ks;
}
}
k++;
// Perform the task indicated by kase.
switch (kase)
{
// Deflate negligible s(p).
case 1:
{
double f = e[p - 2];
e[p - 2] = 0.0;
for (int j = p - 2; j >= k; j--)
{
double t = Functions.Hypotenuse(s[j], f);
double cs = s[j] / t;
double sn = f / t;
s[j] = t;
if (j != k)
{
f = (-sn) * e[j - 1];
e[j - 1] = cs * e[j - 1];
}
if (wantv)
{
for (int i = 0; i < n; i++)
{
t = cs * V[i, j] + sn * V[i, p - 1];
V[i, p - 1] = (-sn) * V[i, j] + cs * V[i, p - 1];
V[i, j] = t;
}
}
}
}
break;
// Split at negligible s(k).
case 2:
{
double f = e[k - 1];
e[k - 1] = 0.0;
for (int j = k; j < p; j++)
{
double t = Functions.Hypotenuse(s[j], f);
double cs = s[j] / t;
double sn = f / t;
s[j] = t;
f = (-sn) * e[j];
e[j] = cs * e[j];
if (wantu)
{
for (int i = 0; i < m; i++)
{
t = cs * U[i, j] + sn * U[i, k - 1];
U[i, k - 1] = (-sn) * U[i, j] + cs * U[i, k - 1];
U[i, j] = t;
}
}
}
}
break;
// Perform one qr step.
case 3:
{
// Calculate the shift.
double scale =
M.Max(M.Max(M.Max(M.Max(M.Abs(s[p - 1]), M.Abs(s[p - 2])), M.Abs(e[p - 2])), M.Abs(s[k])),
M.Abs(e[k]));
double sp = s[p - 1] / scale;
double spm1 = s[p - 2] / scale;
double epm1 = e[p - 2] / scale;
double sk = s[k] / scale;
double ek = e[k] / scale;
double b = ((spm1 + sp) * (spm1 - sp) + epm1 * epm1) / 2.0;
double c = (sp * epm1) * (sp * epm1);
double shift = 0.0;
if ((b != 0.0) | (c != 0.0))
{
shift = M.Sqrt(b * b + c);
if (b < 0.0)
{
shift = -shift;
}
shift = c / (b + shift);
}
double f = (sk + sp) * (sk - sp) + shift;
double g = sk * ek;
// Chase zeros.
for (int j = k; j < p - 1; j++)
{
double t = Functions.Hypotenuse(f, g);
double cs = f / t;
double sn = g / t;
if (j != k)
{
e[j - 1] = t;
}
f = cs * s[j] + sn * e[j];
e[j] = cs * e[j] - sn * s[j];
g = sn * s[j + 1];
s[j + 1] = cs * s[j + 1];
if (wantv)
{
for (int i = 0; i < n; i++)
{
t = cs * V[i, j] + sn * V[i, j + 1];
V[i, j + 1] = (-sn) * V[i, j] + cs * V[i, j + 1];
V[i, j] = t;
}
}
t = Functions.Hypotenuse(f, g);
cs = f / t;
sn = g / t;
s[j] = t;
f = cs * e[j] + sn * s[j + 1];
s[j + 1] = (-sn) * e[j] + cs * s[j + 1];
g = sn * e[j + 1];
e[j + 1] = cs * e[j + 1];
if (wantu && (j < m - 1))
{
for (int i = 0; i < m; i++)
{
t = cs * U[i, j] + sn * U[i, j + 1];
U[i, j + 1] = (-sn) * U[i, j] + cs * U[i, j + 1];
U[i, j] = t;
}
}
}
e[p - 2] = f;
iter = iter + 1;
}
break;
// Convergence.
case 4:
{
// Make the singular values positive.
if (s[k] <= 0.0)
{
s[k] = (s[k] < 0.0 ? -s[k] : 0.0);
if (wantv)
{
for (int i = 0; i <= pp; i++)
{
V[i, k] = -V[i, k];
}
}
}
// Order the singular values.
while (k < pp)
{
if (s[k] >= s[k + 1])
{
break;
}
double t = s[k];
s[k] = s[k + 1];
s[k + 1] = t;
if (wantv && (k < n - 1))
{
for (int i = 0; i < n; i++)
{
t = V[i, k + 1];
V[i, k + 1] = V[i, k];
V[i, k] = t;
}
}
if (wantu && (k < m - 1))
{
for (int i = 0; i < m; i++)
{
t = U[i, k + 1];
U[i, k + 1] = U[i, k];
U[i, k] = t;
}
}
k++;
}
iter = 0;
p--;
}
break;
}
}
//Complete -- Return SVD Result
return(new SvdResult(U, s, V));
}
private void setCost(Vector3 destination)
{
this.cost = (int)(Math.Abs(this.pos.x - destination.x) + Math.Abs(this.pos.z - destination.z));
}
/// <summary>
/// Prowadzi ostrzal samolotu.
/// </summary>
public override void Fire(int time)
{
base.Fire(time);
//jesli nie jest zniszczony i samolot jeszcze jest caly
if (!IsDestroyed && UserPlaneNotYetDestroyed)
{
bool fireCondition = IsFireConditionMet;
//wyliczam kat tylko jesli nie obracamy
if (!changeDirection.HasValue)
{
float angleBefore = angle;
SetAngle(time);
if (adjustingBarrelAfterDirectionChange)
{
float diff = angle - angleBefore;
angle = angleBefore + diff * BarrelAdjustmentSpeed * time;
// Console.WriteLine("angle:" + angle + "; diff:" + diff);
if (Math.Abs(angle - angleBefore) < Epislon)
{
adjustingBarrelAfterDirectionChange = false;
}
}
}
else
{
adjustingBarrelAfterDirectionChange = false; // przerwij
}
//jesli uplynal czas od ostatniego strzalu.
if (currentTime > GameConsts.FlakBunker.FireDelay)
{
//jesli samolot jest w polu razenia.
if (fireCondition)
{
//zadaje uszkodzenia.
float localAngle = angle;
if (localAngle > Mogre.Math.HALF_PI)
{
localAngle = Mogre.Math.PI - localAngle;
}
FlakBullet bullet = weaponManager.FlakFire(refToLevel.UserPlane, localAngle);
//powiadamia controler o strzale.
refToLevel.Controller.OnBunkerFire(this, refToLevel.UserPlane, false);
// Console.WriteLine("ANGLE: "+angle);
//Zeruje licznik. Czekam kolejna sekunde.
currentTime = 0;
}
}
else //zwiekszam odstep czasu od ostatniego strzalu
{
currentTime += time;
}
}
}