public Vector5 rotatePointAroundSpace(Vector5 point, float angle, RotationSpace5D space)
{
int[] spaceCoordinates = Rotation5D.getRotationSpaceIndices(space);
int[] freeCoordinates = new int[2];
int a = 0;
for (int i = 0; i < 5; i++)
{
if (!spaceCoordinates.Contains(i))
{
freeCoordinates [a] = i;
a++;
}
}
float X = point[freeCoordinates [0]];
float Y = point[freeCoordinates [1]];
float newX = X * Mathf.Cos(angle) - Y * Mathf.Sin(angle);
float newY = X * Mathf.Sin(angle) + Y * Mathf.Cos(angle);
point [freeCoordinates [0]] = newX;
point [freeCoordinates [1]] = newY;
return(point);
}
public Matrix4x5(Vector5 <TSource, TSourceNumber> x, Vector5 <TSource, TSourceNumber> y, Vector5 <TSource, TSourceNumber> z, Vector5 <TSource, TSourceNumber> w)
{
X = x;
Y = y;
Z = z;
W = w;
}
public override bool Equals(object obj)
{
if (obj is Vector5)
{
Vector5 A = (Vector5)obj;
if (A.largeRow != this.largeRow)
{
return(false);
}
else if (A.largeColumn != this.largeColumn)
{
return(false);
}
else if (A.smallRow != this.smallRow)
{
return(false);
}
else if (A.smallColumn != this.smallColumn)
{
return(false);
}
else if (A.wonBy != this.wonBy)
{
return(false);
}
else
{
return(true);
}
}
else
{
throw new InvalidCastException("The object 'obj' is not castable to the type 'Vector5'");
}
}
private IEnumerator WORKProgram(Vector5 T)
{
workProgram = true;
for (int i = 0; programVectors.Length > i; i++)
{
KinematicField.InpVector = programVectors[i];
//SetOptionPoint(programVectors[i]);
if (programVectors.Length - 1 > i)
{
distance = Vector5.Distance(programVectors[i], programVectors[i + 1]) * 100;
time = distance / targetProperties.speed;
yield return(new WaitForSeconds(time));
}
else
{
targetPoint = programVectors[i];
}
if (workProgram == false)
{
break;
}
}
workProgram = false;
}
public void SetInputField(Vector5 vector)
{
inputFieldX.text = vector.X.ToString();
inputFieldY.text = vector.Y.ToString();
inputFieldZ.text = vector.Z.ToString();
inputFieldA.text = vector.A.ToString();
inputFieldB.text = vector.B.ToString();
}
public static float Distance(Vector5 a, Vector5 b)
{
return((float)Math.Sqrt(((b.largeRow - a.largeRow) * (b.largeRow - a.largeRow)) +
((b.largeColumn - a.largeColumn) * (b.largeColumn - a.largeColumn)) +
((b.smallRow - a.smallRow) * (b.smallRow - a.smallRow)) +
((b.smallColumn - a.smallColumn) * (b.smallColumn - a.smallColumn)) +
((b.wonBy - a.wonBy) * (b.wonBy - a.wonBy))));
}
public static Vector5 Slerp(Vector5 from, Vector5 to, float t)
{
Vector5 A = from;
Vector5 B = to;
A.largeRow = A.largeRow % 360;
A.largeColumn = A.largeColumn % 360;
A.smallRow = A.smallRow % 360;
A.smallColumn = A.smallColumn % 360;
A.wonBy = A.wonBy % 360;
B.largeRow = B.largeRow % 360;
B.largeColumn = B.largeColumn % 360;
B.smallRow = B.smallRow % 360;
B.smallColumn = B.smallColumn % 360;
B.wonBy = B.wonBy % 360;
if (A.largeRow > 180)
{
A.largeRow = -(360 - A.largeRow);
}
if (A.largeColumn > 180)
{
A.largeColumn = -(360 - A.largeColumn);
}
if (A.smallRow > 180)
{
A.smallRow = -(360 - A.smallRow);
}
if (A.smallColumn > 180)
{
A.smallColumn = -(360 - A.smallColumn);
}
if (A.wonBy > 180)
{
A.wonBy = -(360 - A.wonBy);
}
if (B.largeRow > 180)
{
B.largeRow = -(360 - B.largeRow);
}
if (B.largeColumn > 180)
{
B.largeColumn = -(360 - B.largeColumn);
}
if (B.smallRow > 180)
{
B.smallRow = -(360 - B.smallRow);
}
if (B.smallColumn > 180)
{
B.smallColumn = -(360 - B.smallColumn);
}
if (B.wonBy > 180)
{
B.wonBy = -(360 - B.wonBy);
}
return(Vector5.Lerp(A, B, t));
}
static public Joint6 InverseKinematicStatic(Vector5 inpVector)
{
// Inp x,y,z, a,b,c
// Out J1, J2 ...
float l1 = 7, l2 = 5, l3 = 6.7f, l4 = 3;
Vector3 InpVector3 = inpVector.GetVector3();
float atan = Mathf.Atan2(InpVector3.x, InpVector3.z);
Vector3 orientVector = new Vector3(l4 * Mathf.Cos(inpVector.A * Mathf.Deg2Rad) * Mathf.Sin(atan),
l4 * Mathf.Sin(inpVector.A * Mathf.Deg2Rad),
l4 * Mathf.Cos(inpVector.A * Mathf.Deg2Rad) * Mathf.Cos(atan)); // вектор подхода n
Vector3 XYZ_1 = InpVector3 - orientVector;
float q1 = Mathf.Atan2(XYZ_1.x, XYZ_1.z); // ??? Mathf.Atan2(XYZ_1.X, XYZ_1.Z)
float q2_acos = Mathf.Acos((Mathf.Pow(XYZ_1.x, 2) + Mathf.Pow(XYZ_1.y - l1, 2) + Mathf.Pow(XYZ_1.z, 2) + Mathf.Pow(l2, 2) - Mathf.Pow(l3, 2))
/ (2 * Mathf.Sqrt(Mathf.Abs(Mathf.Pow(XYZ_1.x, 2) + Mathf.Pow(XYZ_1.y - l1, 2) + Mathf.Pow(XYZ_1.z, 2))) * l2));
float q2 = Mathf.PI / 2 - Mathf.Atan2(XYZ_1.y - l1, Mathf.Sqrt(Mathf.Pow(XYZ_1.x, 2) + Mathf.Pow(XYZ_1.z, 2)));
if (!float.IsNaN(q2_acos))
{
q2 = Mathf.PI / 2 - Mathf.Atan2(XYZ_1.y - l1, Mathf.Sqrt(Mathf.Pow(XYZ_1.x, 2) + Mathf.Pow(XYZ_1.z, 2))) - q2_acos;
}
float q3_acos = (-Mathf.Pow(XYZ_1.x, 2) - Mathf.Pow(XYZ_1.y - l1, 2) - Mathf.Pow(XYZ_1.z, 2) + Mathf.Pow(l2, 2) + Mathf.Pow(l3, 2)) / (2 * l3 * l2);
float q3 = Mathf.PI - Mathf.Acos(q3_acos);
if (q3_acos > 1)
{
q3 = Mathf.PI;
}
if (q3_acos < -1 || float.IsNaN(q3))
{
q3 = 0;
}
Vector3 orientVector_1 = new Vector3(Mathf.Sin(q2 + q3) * Mathf.Sin(q1),
Mathf.Cos(q2 + q3),
Mathf.Sin(q2 + q3) * Mathf.Cos(q1)); // вектор подхода n-1
float q4 = Vector3.Angle(orientVector_1, orientVector) * Mathf.Deg2Rad;
float sign = orientVector_1.y * Mathf.Sqrt(Mathf.Pow(orientVector.x, 2) + Mathf.Pow(orientVector.z, 2)) -
orientVector.y * Mathf.Sqrt(Mathf.Pow(orientVector_1.x, 2) + Mathf.Pow(orientVector_1.z, 2)); // считаем определитель в плоскости
q4 = q4 * Mathf.Sign(sign);
float q5 = inpVector.B * Mathf.Deg2Rad;
return(new Joint6(q1 * Mathf.Rad2Deg, q2 * Mathf.Rad2Deg, q3 * Mathf.Rad2Deg, q4 * Mathf.Rad2Deg, q5 * Mathf.Rad2Deg, inpVector.S));
}
public static float Magnitude(Vector5 a)
{
return(Mathf.Sqrt(
Mathf.Pow(a.largeRow, 2) +
Mathf.Pow(a.largeColumn, 2) +
Mathf.Pow(a.smallRow, 2) +
Mathf.Pow(a.smallColumn, 2) +
Mathf.Pow(a.wonBy, 2)));
}
public static Vector5 Lerp(Vector5 from, Vector5 to, float t)
{
return(new Vector5(
Mathf.Lerp(from.largeRow, to.largeRow, t),
Mathf.Lerp(from.largeColumn, to.largeColumn, t),
Mathf.Lerp(from.smallRow, to.smallRow, t),
Mathf.Lerp(from.smallColumn, to.smallColumn, t),
Mathf.Lerp(from.wonBy, to.wonBy, t)));
}
public static Vector5 operator *(Vector5 left, Vector5 right)
{
Vector5 v5 = new Vector5();
for (int i = 0; i < 5; i++)
{
v5.components [i] = left.components [i] * right.components[i];
}
return(v5);
}
public void InitPointGO(PrefabCollection prefabCollection, GameObject trajectory, Vector2Int Num)
{
ItemPoint = Instantiate(prefabCollection.ItemPrefab);
ItemPoint.transform.SetParent(trajectory.transform, false);
ItemPoint.gameObject.GetComponentInChildren <Text>().text = " point";
this.Num = Num;
//---------------------------------------------------//
ScencePoint = Instantiate(prefabCollection.taregetPointPrefab);
TransformVector = new Vector5(new Vector3(0, 14.63f, 5.26f), new Vector2(25, 0), 0);
}
public static Vector5 operator +(Vector5 left, Vector5 right)
{
Vector5 v5 = Vector5.zero;
for (int i = 0; i < 5; i++)
{
v5.components [i] = left.components [i] + right.components [i];
}
return(v5);
}
public static Vector5 operator *(Vector5 left, float right)
{
Vector5 v5 = Vector5.zero;
for (int i = 0; i < 5; i++)
{
v5.components [i] = left.components [i] * right;
}
return(v5);
}
public static Vector5 Normalize(Vector5 a)
{
float Mag = Vector5.Magnitude(a);
return(new Vector5(
a.largeRow / Mag,
a.largeColumn / Mag,
a.smallRow / Mag,
a.smallColumn / Mag,
a.wonBy / Mag));
}
public Vector5 getWorldPosition(Vector5 localPosition)
{
Vector5 worldPos = localPosition * scale;
worldPos = rotatePointAroundSpace(worldPos, rotation.XYZRotation, RotationSpace5D.XYZ);
worldPos = rotatePointAroundSpace(worldPos, rotation.XYWRotation, RotationSpace5D.XYW);
worldPos = rotatePointAroundSpace(worldPos, rotation.XYVRotation, RotationSpace5D.XYV);
worldPos = rotatePointAroundSpace(worldPos, rotation.XZWRotation, RotationSpace5D.XZW);
worldPos = rotatePointAroundSpace(worldPos, rotation.XZVRotation, RotationSpace5D.XZV);
worldPos = rotatePointAroundSpace(worldPos, rotation.XWVRotation, RotationSpace5D.XWV);
return(worldPos + position);
}
public Vector4 GetProjectedPoint(Vector5 point)
{
switch (projectionType)
{
case ProjectionType.Orthographic:
return(OrthographicProjection(point));
case ProjectionType.Perspective:
return(PerspectiveProjection(point));
default:
return(Vector4.zero);
}
}
Vector4 PerspectiveProjection(Vector5 point)
{
if (point [4] == projectionPoint [4])
{
return(new Vector4(Mathf.Infinity, Mathf.Infinity, Mathf.Infinity, Mathf.Infinity));
}
Vector5 S = point;
Vector5 D = projectionPoint - S;
Vector5 PS = new Vector5(new float[] { 0, 0, 0, 0, projectionSpacePointV });
// S[W] + k * D[W] = PS[W]
float k = (PS[4] - S [4]) / D [4];
Vector5 newPoint = S + k * D;
return(new Vector4(newPoint.x, newPoint.y, newPoint.z, newPoint.w));
}
public override Vector5[] GetApproximatedArray()
{
Vector5[] vector5s = new Vector5[splineVectors.Length];
vector5s[0] = pointList[0].TransformVector;
for (int i = 0; i < pointList.Count - 1; i++)
{
for (int j = 1; j <= resolution && j * (i + 1) < vector5s.Length; j++)
{
vector5s[j * (i + 1) + (resolution - j) * i].SetVector5(splineVectors[j * (i + 1) + (resolution - j) * i],
Vector2.Lerp(pointList[i].TransformVector.GetVector2(), pointList[i + 1].TransformVector.GetVector2(), j / (float)resolution),
Mathf.Lerp(pointList[i].TransformVector.S, pointList[i + 1].TransformVector.S, j / (float)resolution));
}
}
return(vector5s);
}
static IEnumerable <Vector5> getCubeVertices(Vector5 currentVertex, int depth)
{
for (int i = 0; i <= 1; i++)
{
Vector5 newVertex = new Vector5(currentVertex.components);
newVertex [depth] = i;
if (depth == 4)
{
yield return(newVertex);
}
else
{
foreach (Vector5 v5 in getCubeVertices(newVertex, depth + 1))
{
yield return(v5);
}
}
}
}
public Vector4 InterpolateVector4(float time)
{
if (_points == null || _points.Count == 0)
{
return(Vector4.zero);
}
if (_points.First().Vector4Point.v >= time)
{
return(_points.First().Vector4Point);
}
if (_points.Last().Vector4Point.v <= time)
{
return(_points.Last().Vector4Point);
}
SearchIndex(time, PropertyType.Vector4, out int l, out int r);
Vector5 pointL = _points[l].Vector4Point;
Vector5 pointR = _points[r].Vector4Point;
float normalTime;
float divisor = pointR.v - pointL.v;
if (divisor != 0)
{
normalTime = (time - pointL.v) / divisor;
}
else
{
normalTime = 0;
}
normalTime = Easings.Interpolate(normalTime, _points[r].Easing);
return(Vector4.LerpUnclamped(pointL, pointR, normalTime));
}
public static PointDefinition DynamicToPointData(dynamic dyn)
{
IEnumerable <List <object> > points = ((IEnumerable <object>)dyn)
?.Cast <List <object> >();
if (points == null)
{
return(null);
}
PointDefinition pointData = new PointDefinition();
foreach (List <object> rawPoint in points)
{
int flagIndex = -1;
int cachedCount = rawPoint.Count;
for (int i = cachedCount - 1; i > 0; i--)
{
if (rawPoint[i] is string)
{
flagIndex = i;
}
else
{
break;
}
}
Functions easing = Functions.easeLinear;
bool spline = false;
List <object> copiedList = rawPoint.ToList();
if (flagIndex != -1)
{
List <string> flags = rawPoint.GetRange(flagIndex, cachedCount - flagIndex).Cast <string>().ToList();
copiedList.RemoveRange(flagIndex, cachedCount - flagIndex);
string easingString = flags.Where(n => n.StartsWith("ease")).FirstOrDefault();
if (easingString != null)
{
easing = (Functions)Enum.Parse(typeof(Functions), easingString);
}
// TODO: add more spicy splines
string splineString = flags.Where(n => n.StartsWith("spline")).FirstOrDefault();
if (splineString == "splineCatmullRom")
{
spline = true;
}
}
if (copiedList.Count() == 2)
{
Vector2 vector = new Vector2(Convert.ToSingle(copiedList[0]), Convert.ToSingle(copiedList[1]));
pointData.LinearAdd(new PointData(vector, easing));
}
else if (copiedList.Count() == 4)
{
Vector4 vector = new Vector4(Convert.ToSingle(copiedList[0]), Convert.ToSingle(copiedList[1]), Convert.ToSingle(copiedList[2]), Convert.ToSingle(copiedList[3]));
pointData.Add(new PointData(vector, easing, spline));
}
else
{
Vector5 vector = new Vector5(Convert.ToSingle(copiedList[0]), Convert.ToSingle(copiedList[1]), Convert.ToSingle(copiedList[2]), Convert.ToSingle(copiedList[3]), Convert.ToSingle(copiedList[4]));
pointData.Add(new PointData(vector, easing));
}
}
return(pointData);
}
public static float Angle(Vector5 a, Vector5 b)
{
Vector5 A = a;
Vector5 B = b;
A.largeRow = A.largeRow % 360;
A.largeColumn = A.largeColumn % 360;
A.smallRow = A.smallRow % 360;
A.smallColumn = A.smallColumn % 360;
A.wonBy = A.wonBy % 360;
B.largeRow = B.largeRow % 360;
B.largeColumn = B.largeColumn % 360;
B.smallRow = B.smallRow % 360;
B.smallColumn = B.smallColumn % 360;
B.wonBy = B.wonBy % 360;
if (A.largeRow > 180)
{
A.largeRow = -(360 - A.largeRow);
}
if (A.largeColumn > 180)
{
A.largeColumn = -(360 - A.largeColumn);
}
if (A.smallRow > 180)
{
A.smallRow = -(360 - A.smallRow);
}
if (A.smallColumn > 180)
{
A.smallColumn = -(360 - A.smallColumn);
}
if (A.wonBy > 180)
{
A.wonBy = -(360 - A.wonBy);
}
if (B.largeRow > 180)
{
B.largeRow = -(360 - B.largeRow);
}
if (B.largeColumn > 180)
{
B.largeColumn = -(360 - B.largeColumn);
}
if (B.smallRow > 180)
{
B.smallRow = -(360 - B.smallRow);
}
if (B.smallColumn > 180)
{
B.smallColumn = -(360 - B.smallColumn);
}
if (B.wonBy > 180)
{
B.wonBy = -(360 - B.wonBy);
}
return(Mathf.Sqrt(((B.largeRow - A.largeRow) * (B.largeRow - A.largeRow)) +
((B.largeColumn - A.largeColumn) * (B.largeColumn - A.largeColumn)) +
((B.smallRow - A.smallRow) * (B.smallRow - A.smallRow)) +
((B.smallColumn - A.smallColumn) * (B.smallColumn - A.smallColumn)) +
((B.wonBy - A.wonBy) * (B.wonBy - A.wonBy))));
}
public static float Dot(Vector5 a, Vector5 b)
{
return((a.largeRow * b.largeRow) + (a.largeColumn * b.largeColumn) + (a.smallRow * b.smallRow) + (a.smallColumn * b.smallColumn) + (a.wonBy * b.wonBy));
}
public void ReadInputField()
{
targetProperties.SetSlider(Kinematic.InverseKinematicStatic(targetProperties.GetInputField()));
targetPoint = targetProperties.GetInputField();
targetProperties.SetProp3Slider(KinematicField.lim3Ext.B, KinematicField.lim3Ext.A);
}
//static public Vector5 SmoothDamp(Vector5 current, Vector5 target, ref Vector5 currentVelocity, float smoothTime)
//{
// Vector3.SmoothDamp(current.GetVector3(), target.GetVector3(), ref currentVelocity.GetVector3(), smoothTime);
// Vector2.SmoothDamp(current, target, ref currentVelocity, smoothTime);
//}
static public float Distance(Vector5 a, Vector5 b)
{
return(Vector3.Distance(a.GetVector3(), b.GetVector3()));
}
internal PointData(Vector5 point, Functions easing = Functions.easeLinear)
{
Vector4Point = point;
Easing = easing;
}
public static float SqrMagnitude(Vector5 a)
{
return(Mathf.Pow(Vector5.Magnitude(a), 2));
}
public static unsafe ref Vector3 cast_vec3(Vector5 s) => ref *(Vector3 *)&s;
public void SetOptionPoint(Vector5 vector5)
{
pointList[idPoint].TransformVector = vector5;
PaintTrajectory();
}
