/// <summary>
/// Konstruktor vektoru
/// </summary>
/// <param name="Copy">Vzor pro nový vektor</param>
public Vector(Vector Copy)
{
if (Copy == null) throw new ArgumentNullException();
t = new double[Copy.Count];
for (int i = 0; i < Copy.Count; i++)
t[i] = Copy[i];
}
/// <summary>
/// Vytvoří těleso s danou geometrií,hmotností a materiálem
/// </summary>
/// <param name="ObjectModel">Model tělesa</param>
/// <param name="ObjectMass">Hmotnost tělesa</param>
/// <param name="ObjectMaterial">Materiál tělesa</param>
public SimObject(Geometry ObjectModel, double ObjectMass, Material ObjectMaterial)
{
if (!ObjectModel.GetType().Attributes.HasFlag(System.Reflection.TypeAttributes.Serializable))
throw new ArgumentException();
model = ObjectModel;
Mass = ObjectMass;
ObjMaterial = ObjectMaterial;
totalForce = Vector.Zero;
totalTorque = Vector.Zero;
LinearVelocity = Vector.Zero;
AngularVelocity = Vector.Zero;
Enabled = true;
NoTranslations = false;
Static = false;
double denom = 0,num = 0,factor = 0;
for (int i = 0, j = 0; i < Model.ObjectGeometry.Length; i++)
{
j = (i + 1) % Model.ObjectGeometry.Length;
factor = Vector.Pow((Vector)Model.ObjectGeometry[j], 2) + Vector.Dot((Vector)Model.ObjectGeometry[j], (Vector)Model.ObjectGeometry[i]) + Vector.Pow((Vector)Model.ObjectGeometry[i], 2);
num += Vector.Cross(((Vector)Model.ObjectGeometry[j]), ((Vector)Model.ObjectGeometry[i])).Magnitude * factor;
denom += Vector.Cross(((Vector)Model.ObjectGeometry[j]), ((Vector)Model.ObjectGeometry[i])).Magnitude;
}
J = num / denom;
}
/// <summary>
/// Vytvoří nové fyzikální pole sil
/// </summary>
/// <param name="Center">Souřadnice centra pole</param>
/// <param name="FieldFunction">Funkce vracející sílu pole na dané těleso</param>
/// <param name="FieldParams">Volitelné parametry pole</param>
public Field(Vector Center, ForceFunction FieldFunction, params object[] FieldParams)
{
if (FieldFunction == null) throw new ArgumentNullException();
F = FieldFunction;
center = Center;
iparams = FieldParams;
enabled = true;
}
/// <summary>
/// Výchozí konstruktor
/// </summary>
/// <param name="ObjA">První účastník kolize</param>
/// <param name="ObjB">Druhý účastník kolize</param>
/// <param name="MinimumTranslation">Vektor minimálního posunutí</param>
public CollisionReport(SimObject ObjA, SimObject ObjB,Vector MinimumTranslation)
{
A = ObjA;
B = ObjB;
MTD = MinimumTranslation;
N = Vector.Unit(MTD);
Prepare();
NAP = ((Vector)Pairs[0].A - A.COG).Perp();
NBP = ((Vector)Pairs[0].B - B.COG).Perp();
RelativeVelocity = (A.LinearVelocity + Vector.Cross(NAP.Perp(), A.AngularVelocity)) - (B.LinearVelocity + Vector.Cross(NBP.Perp(), B.AngularVelocity));
}
/// <summary>
/// Aplikuje 2D transformaci danou maticí na body v homogenní soustavě souřadné
/// </summary>
/// <param name="Points">Body, na které má být aplikována transformace</param>
public static System.Drawing.PointF[] TransformPoints(Matrix Transform, params System.Drawing.PointF[] Points)
{
if (Transform.Order != 3) throw new MatrixException();
for (int i = 0; i < Points.Length; i++)
{
Vector T = new Vector(Points[i].X, Points[i].Y, 1);
T = (Transform * T.ToMatrix(MatrixInitType.VectorsAreColumns)).ToVector();
Points[i].X = (float)T[0]; Points[i].Y = (float)T[1];
}
return Points;
}
/// <summary>
/// Vytvoří 2D transformační matici rotace podle dané osy o daný úhel (pro homogenní souřadnice)
/// </summary>
/// <param name="Angle">Úhel otočení v radiánech</param>
/// <param name="Axis">Osa otočení</param>
/// <returns>Transformační matice rotace</returns>
public static Matrix Rotate(double Angle, Vector Axis)
{
Matrix Rot = new Matrix(3);
Rot[0, 0] = Math.Cos(Angle); Rot[0, 1] = -Math.Sin(Angle); Rot[0, 2] = -Axis[0] * Math.Cos(Angle) + Axis[1] * Math.Sin(Angle) + Axis[0];
Rot[1, 0] = Math.Sin(Angle); Rot[1, 1] = Math.Cos(Angle); Rot[1, 2] = -Axis[0] * Math.Sin(Angle) - Axis[1] * Math.Cos(Angle) + Axis[1];
Rot[2, 0] = 0; Rot[2, 1] = 0; Rot[2, 2] = 1;
return Rot ;
}
/// <summary>
/// Vytvoří 2D transformační matici škálování a translace s daným škálovacím faktorem a posunutím
/// </summary>
/// <param name="Factor">Škálovací faktor</param>
/// <param name="At">Vektor posunutí</param>
/// <returns>Transformační matice škálování a translace</returns>
public static Matrix Scale(Vector Factor, Vector At)
{
Matrix Scale = new Matrix(3);
Scale[0, 0] = Factor[0]; Scale[0, 1] = 0; Scale[0, 2] = -At[0] * Factor[0] + At[0] ;
Scale[1, 0] = 0; Scale[1, 1] = Factor[1]; Scale[1, 2] = -At[1] * Factor[1] + At[1];
Scale[2, 0] = 0; Scale[2, 1] = 0; Scale[2, 2] = 1;
return Scale;
}
/// <summary>
/// Převede daný vektor do báze zadané bázovými vektory jakožto sloupce dané diagonální matice
/// </summary>
/// <param name="B">Matice báze</param>
/// <param name="A">Vektor, který se má převést</param>
/// <returns>Vektor A v bázi B</returns>
public static Vector ToBasis(Matrix B, Vector A)
{
if (!B.Diagonal || B.Columns != A.Count)
throw new MatrixException();
Vector Ret = new Vector(A.Count);
for (int i = 0, j = 0; i < B.Dimension; i += B.Columns + 1, j++)
Ret[j] = B[i] * A[j];
return Ret;
}
/// <summary>
/// Získá řádek matice jako vektor
/// </summary>
/// <param name="Row">Souřadnice řádku</param>
/// <returns>Vektor</returns>
public Vector GetRowAsVector(int Row)
{
if (Row >= rows) throw new MatrixException();
Vector Ret = new Vector(cols);
for (int i = 0; i < cols; i++)
{
Ret[i] = this[Row, i];
}
return Ret;
}
/// <summary>
/// Aplikuje sílu na těleso
/// </summary>
/// <param name="Force">Vektor síly</param>
/// <param name="Origin">Působiště síly</param>
public void ApplyForce(Vector Force,Vector Origin)
{
if (Force.IsNull || Force.IsNaN) return;
totalForce += Force;
totalTorque += Vector.Cross(GetTorqueIntersection(Force, Model.ProjectToObject(Origin)) - RotationPoint, Force);
}
/// <summary>
/// Spočítá vzdálenost mezi dvěma body
/// </summary>
/// <param name="a">Bod A</param>
/// <param name="b">Bod B</param>
/// <returns>Vzdálenost |AB|</returns>
public static double PointDistance(Vector a, Vector b)
{
if (a.Count != b.Count) throw new ArgumentException();
double ret = 0;
for (int i = 0; i < a.Count; i++)
ret += Math.Pow(a[i] - b[i], 2);
return Math.Sqrt(ret);
}
/// <summary>
/// Provede převod vektoru z metrů na pixely nebo z pixelů na metry podle daného rozlišení světa
/// </summary>
/// <param name="Input">Vstupní číslo</param>
/// <param name="Type">Typ převodu</param>
/// <returns>Převedené číslo</returns>
public Vector Convert(Vector Input, ConversionType Type)
{
if (Type == ConversionType.MetersToPixels)
return Input * r;
else return Input / r;
}
/// <summary>
/// Najde první nejbližší těžiště objektu ve světě k dané pozici
/// </summary>
/// <param name="Position">Pozice</param>
/// <param name="SkipStatic">Indikuje, zda přeskočit statické objekty</param>
/// <returns>Nejbližší objekt</returns>
public SimObject NearestObject(Vector Position,bool SkipStatic = true)
{
double distance = Double.PositiveInfinity;
int index = -1;
if (Objs.Count == 0)
throw new InvalidOperationException();
for (int i = 0; i < Objs.Count; i++)
{
if (((SimObject)Objs[i]).Static && SkipStatic) continue;
double dist = (((SimObject)Objs[i]).Model.Position - Position).Magnitude;
if (dist == 0) return Objs[i] as SimObject;
if (dist < distance)
{
distance = dist;
index = i;
}
}
if (index < 0) return null;
return Objs[index] as SimObject;
}
/// <summary>
/// Převede pole třídy PointF na pole vektorů
/// </summary>
/// <param name="Points">Pole bodů</param>
/// <returns>Pole vektorů</returns>
public static Vector[] PointsToVectors(System.Drawing.PointF[] Points)
{
Vector[] Out = new Vector[Points.Length];
for (int i = 0; i < Points.Length; i++)
Out[i] = new Vector((double)Points[i].X, (double)Points[i].Y);
return Out;
}
/// <summary>
/// Převede vektory na pole třídy Point
/// </summary>
/// <param name="Points">Pole vektorů</param>
/// <returns>Pole Point</returns>
public static System.Drawing.Point[] VectorsToPoints(Vector[] Vectors)
{
System.Drawing.Point[] Out = new System.Drawing.Point[Vectors.Length];
for (int i = 0; i < Vectors.Length; i++)
Out[i] = new System.Drawing.Point((int)Vectors[i][0], (int)Vectors[i][1]);
return Out;
}
/// <summary>
/// Spočítá "skalární mocninu" vektoru
/// </summary>
/// <param name="v">Vektor</param>
/// <param name="n">Mocnitel</param>
/// <returns>Velikost vektoru na ntou</returns>
public static double Pow(Vector v, uint n)
{
return Math.Pow(v.Magnitude, n);
}
/// <summary>
/// Udělá z daného vektoru jednotkový vektor ukazující pouze směr
/// </summary>
/// <param name="v">Vstupní vektor</param>
/// <returns>Jednotkový vektor</returns>
public static Vector Unit(Vector v)
{
if (v.IsNull) return new Vector(v.Count);
Vector Ret = new Vector(v.Count);
for (int i = 0; i < v.Count;i++)
Ret[i] = v[i]/v.Magnitude;
return Ret;
}
/// <summary>
/// Usekne desetinnou část všech prvků ve vektoru
/// </summary>
/// <param name="v">Vektor</param>
/// <returns>Vektor se složkami bez desetinných částí</returns>
public static Vector Truncate(Vector v)
{
Vector Ret = new Vector(v.Count);
if (v.IsNull) return Ret;
for (int i = 0; i < v.Count; i++)
Ret[i] = Math.Truncate(v[i]);
return Ret;
}
/// <summary>
/// Vytvoří 2D transformační matici posunutí o daný vektor (pro homogenní souřadnice)
/// </summary>
/// <param name="To">Vektor posunutí</param>
/// <returns>Transformační matice translace</returns>
public static Matrix Translate(Vector To)
{
Matrix Trans = new Matrix(3);
Trans[0, 0] = 1; Trans[0, 1] = 0; Trans[0, 2] = To[0];
Trans[1, 0] = 0; Trans[1, 1] = 1; Trans[1, 2] = To[1];
Trans[2, 0] = 0; Trans[2, 1] = 0; Trans[2, 2] = 1;
return Trans;
}
/// <summary>
/// Vytvoří fyzický model tělesa jako objekt z daných vertexů
/// </summary>
/// <param name="Vertices">Vertexy tělesa</param>
/// <param name="InitPosition">Počáteční poloha tělesa</param>
/// <param name="COG">Těžiště tělesa</param>
public Geometry(PointF[] Vertices,PointF InitPosition, PointF? COG)
{
if (Vertices == null || Vertices.Length < 3) throw new ArgumentException();
surf = vol = angle = 0;
scale = 1.0f;
desc = AnalyzeVertexGroup(Vertices);
geom = new PointF[Vertices.Length];
Vertices.CopyTo(geom, 0);
if (COG.HasValue)
center = (Vector)COG;
else center = (Vector)desc.Centroid;
Nail = (Vector)InitPosition;
Position = (Vector)InitPosition;
}
/// <summary>
/// Vytvoří 2D transformační matici součastného posunutí a rotace okolo daného bodu
/// </summary>
/// <param name="MoveBy">Vektor posunutí</param>
/// <param name="RotateBy">Úhel otočení v radiánech</param>
/// <param name="RotationAxis">Osa otočení</param>
/// <returns>Transformační matice posunutí a rotace</returns>
public static Matrix TranslateAndRotate(Vector MoveBy, double RotateBy, Vector RotationAxis)
{
Matrix Rot = new Matrix(3);
Rot[0, 0] = Math.Cos(RotateBy); Rot[0, 1] = -Math.Sin(RotateBy); Rot[0, 2] = -RotationAxis[0] * Math.Cos(RotateBy) + RotationAxis[1] * Math.Sin(RotateBy) + RotationAxis[0] + MoveBy[0];
Rot[1, 0] = Math.Sin(RotateBy); Rot[1, 1] = Math.Cos(RotateBy); Rot[1, 2] = -RotationAxis[0] * Math.Sin(RotateBy) - RotationAxis[1] * Math.Cos(RotateBy) + RotationAxis[1] + MoveBy[1];
Rot[2, 0] = 0; Rot[2, 1] = 0; Rot[2, 2] = 1;
return Rot;
}
/// <summary>
/// Provede projekci libovolného bodu na povrch tělesa
/// </summary>
/// <param name="ExternalPoint">Bod ležící mimo těleso</param>
/// <returns>Bod na tělese</returns>
public Vector ProjectToObject(Vector ExternalPoint)
{
if (ExternalPoint == center) return center;
int a = 0, b = 1;
Vector Ret = null;
double ad = double.PositiveInfinity, bd = 0, cd = double.PositiveInfinity, dist = 0;
for (int i = 0; i < ObjectGeometry.Length; i++)
{
dist = Math.Sqrt(Math.Pow(ExternalPoint[0] - ObjectGeometry[i].X, 2) + Math.Pow(ExternalPoint[1] - ObjectGeometry[i].Y, 2));
if (dist < ad)
{
bd = ad;
ad = dist;
b = a;
a = i;
}
else if (dist < bd)
{
bd = dist;
b = i;
}
}
float ax = ObjectGeometry[b].X - ObjectGeometry[a].X, ay = ObjectGeometry[b].Y - ObjectGeometry[a].Y,
x = ObjectGeometry[a].X, y = ObjectGeometry[a].Y,max = (float)Math.Sqrt(ax * ax + ay * ay);
while (x != ObjectGeometry[b].X || y != ObjectGeometry[b].Y)
{
dist = Geometry.PointDistance(new PointF(x,y), (PointF)ExternalPoint);
if (dist < cd)
{
cd = dist;
Ret = new Vector(x,y,0);
}
if (dist > cd) break;
x += ax / max;
y += ay / max;
}
return Ret;
}
/// <summary>
/// Získá bod ve kterém se protíná prodloužený vektor síly a vektor ramene síly
/// </summary>
/// <param name="Force">Síla působící na těleso</param>
/// <param name="Origin">Působiště síly</param>
/// <returns>Průsečík ramene a síly</returns>
public Vector GetTorqueIntersection(Vector Force,Vector Origin)
{
if (Force.IsNull) return COG;
Vector P = Vector.Zero;
double c = (Force[0] * (RotationPoint[0] - Origin[0]) + Force[1] * (RotationPoint[1] - Origin[1])) / Math.Pow(Force.Magnitude, 2);
P[0] = Origin[0] + Force[0] * c;
P[1] = Origin[1] + Force[1] * c;
P[2] = 0;
if (Vector.PointDistance(P, RotationPoint) < 3) return RotationPoint;
return P;
}
/// <summary>
/// Provede skalární součin dvou vektorů
/// </summary>
/// <param name="v">První vektor</param>
/// <param name="u">Druhé vektor</param>
/// <returns>Skalár</returns>
public static double Dot(Vector v, Vector u)
{
if (v.Count != u.Count) throw new ArgumentException();
double Ret = 0;
for (int i = 0; i < v.Count; i++)
Ret += v[i] * u[i];
return Ret;
}
/// <summary>
/// Vytvoří fyzikální svět
/// </summary>
/// <param name="WorldOrientation">Orientace os zobrazovacího zařízení</param>
/// <param name="WorldGravity">Gravitační zrychlení (v jednotkách SI)</param>
/// <param name="WorldDiameter">Poloměr světa v pixelech (maximální vzdálnost tělesa od počátku souřadnic)</param>
/// <param name="WorldResolution">Počet pixelů který představuje jeden fyzický metr</param>
public World(Matrix WorldOrientation, Vector WorldGravity, double WorldDiameter, double WorldResolution = 30)
{
Fields = new List<Field>();
Objs = new List<SimObject>();
lv = new Vector(0, WorldDiameter, 0);
SimLock = new object();
csolve = new CollisionSolver(this);
b = WorldOrientation;
maxRad = WorldDiameter;
Gravity = Vector.ToBasis(b, WorldGravity) * WorldResolution;
simulationTime = 0;
r = WorldResolution;
Delta = 0.01;
paused = false;
DeleteOutOfBounds = false;
}
/// <summary>
/// Zaokrouhlí všechny prvky vektoru s danou přesností
/// </summary>
/// <param name="v">Vektor k zaokrouhlení</param>
/// <param name="decimals">Přesnost zaokrouhlování</param>
/// <returns>Zaokrouhlený vektor</returns>
public static Vector Round(Vector v,int decimals)
{
Vector Ret = new Vector(v.Count);
if (v.IsNull) return Ret;
for (int i = 0; i < v.Count; i++)
Ret[i] = Math.Round(v[i],decimals);
return Ret;
}
/// <summary>
/// Projektuje těleso na danou osu
/// </summary>
/// <param name="Axis">Osa projekce</param>
/// <param name="min">Dolní mez intervalu</param>
/// <param name="max">Horní mez intervalu</param>
public void ProjectToAxis(Vector Axis, ref double min, ref double max)
{
PointF[] axProj = BoundingBox;
min = max = Vector.Dot(Axis, (Vector)axProj[0]);
for (int i = 1; i < axProj.Length; i++)
{
double d = Vector.Dot(Axis, (Vector)axProj[i]);
if (d < min)
min = d;
else if (d > max)
max = d;
}
}
/// <summary>
/// Zjistí, zda je jedno těleso od druhého odděleno danou osou
/// </summary>
/// <param name="Axis">Osa oddělení</param>
/// <param name="ObjectA">Objekt A</param>
/// <param name="ObjectB">Objekt B</param>
/// <param name="MTD">Minimální vektor oddělení</param>
/// <returns>True pokud je odděleno, False pokud nikoliv</returns>
public static bool SeparatedByAxis(Vector Axis, Geometry ObjectA, Geometry ObjectB,ref Vector MTD)
{
double minA = 0, maxA = 0, minB = 0, maxB = 0;
ObjectA.ProjectToAxis(Axis, ref minA, ref maxA);
ObjectB.ProjectToAxis(Axis, ref minB, ref maxB);
double d0 = (maxB - minA);
double d1 = (minB - maxA);
if(d0 < 0.0f || d1 > 0.0f) return true;
double overlap = (d0 < -d1) ? d0 : d1;
Vector Sep = Axis * (overlap / Vector.Pow(Axis, 2));
if (MTD == null || MTD.IsNull || Sep.Magnitude < MTD.Magnitude)
MTD = Sep;
return false;
}
internal PointF[] SupportPoints(Vector Axis)
{
double min = -1.0f;
const double threshold = 1.0E-1;
PointF[] supGeom = BoundingBox;
List<PointF> sp = new List<PointF>();
for (int i = 0; i < supGeom.Length; i++)
{
double t = Vector.Dot(Axis,(Vector)supGeom[i]);
if (t < min || i == 0)
min = t;
}
for (int i = 0; i < supGeom.Length; i++)
{
double t = Vector.Dot(Axis,(Vector)supGeom[i]);
if (t < min + threshold)
{
sp.Add(supGeom[i]);
if (sp.Count == 2) break;
}
}
return sp.ToArray();
}
/// <summary>
/// Získá sloupec matice jako vektor
/// </summary>
/// <param name="Column">Souřadnice sloupce</param>
/// <returns>Vektor</returns>
public Vector GetColumnAsVector(int Column)
{
if (Column >= cols) throw new MatrixException();
Vector Ret = new Vector(rows);
for (int i = 0; i < rows; i++)
{
Ret[i] = this[i, Column];
}
return Ret;
}
