private float ComputeMecanumDriveMotorSpeed(DenseVector desiredMovement, VectorComponent wheelForceVectorInput)
{
var wheelForceVector = new DenseVector(new[] { wheelForceVectorInput.X, wheelForceVectorInput.Y });
var wheelForceVectorUnit = wheelForceVector.Normalize(2);
var scale = wheelForceVectorUnit.DotProduct(desiredMovement);
// Console.WriteLine(wheelForceVector + " " + wheelForceVectorUnit + " " + desiredMovement + " " + scale);
return(scale);
}
private void CalculateRotationMatrix()
{
Vector3D startPos = startNode.Position, endPos = endNode.Position;
double dx, dy, dz;
double nx, ny, nz;
double theta, alpha1, alpha2;
dx = endPos.x - startPos.x;
dy = endPos.y - startPos.y;
dz = endPos.z - startPos.z;
alpha1 = Math.Atan2(dy, dx);
alpha2 = Math.Atan2(dz, Math.Sqrt(Math.Pow(dx, 2) + Math.Pow(dy, 2)));
DenseVector n = (new DenseVector(new double[] { 0, -1.0 * Math.Sin(alpha2), Math.Sin(alpha1) * Math.Cos(alpha2) }));
n = DenseVector.OfVector(n.Normalize(2));
nx = n[0];
ny = n[1];
nz = n[2];
theta = Math.Acos(Math.Cos(alpha1) * Math.Cos(alpha2));
//Mount quaternion parts
double q1 = Math.Cos(theta / 2);
double q2 = nx * Math.Sin(theta / 2);
double q3 = ny * Math.Sin(theta / 2);
double q4 = nz * Math.Sin(theta / 2);
DenseMatrix q = new DenseMatrix(1, 3, new double[] { q2, q3, q4 });
DenseMatrix qT = new DenseMatrix(3, 1, new double[] { q2, q3, q4 });
DenseMatrix I = DenseMatrix.CreateIdentity(3);
DenseMatrix qS = new DenseMatrix(3);
qS[0, 1] = -q4;
qS[0, 2] = q3;
qS[1, 2] = -q2;
qS[1, 0] = q4;
qS[2, 0] = -q3;
qS[2, 1] = q2;
DenseMatrix sub_RotationMatrix = (Math.Pow(q1, 2) - (q * qT)[0, 0]) * I + 2 * qT * q + 2 * q1 * qS;
DenseMatrix temp_RotationMatrix = new DenseMatrix(nodesAmount * 6);
for (int i = 0; i < nodesAmount * 6; i += 3)
{
temp_RotationMatrix.SetSubMatrix(i, i, sub_RotationMatrix);
}
RotationMatrix = temp_RotationMatrix;
}
private Vector <double> GetUnitVector(WiimoteLib.Point pixelcoords)
{
int PIXELS_X = 1024;
int PIXELS_Y = 768;
//const double FOV_X = .578; unused
int pxFromOriginX = pixelcoords.X - (PIXELS_X / 2);
int pxFromOriginY = pixelcoords.Y - (PIXELS_Y / 2);
double pxFromOriginZ = PIXELS_Z; //PIXELS_X / Math.Tan(FOV_X / 2);
var unitVector = new DenseVector(new double[] { pxFromOriginX, pxFromOriginY, pxFromOriginZ });
return(unitVector.Normalize(2));
}
private void CreateOffsetVectors()
{
foreach (var indexNormal in indexToNormal)
{
int index = indexNormal.Key;
Vector <double>[] distinctNormals = indexNormal.Value.Distinct(new VectorEqualityComparer()).ToArray();
Vector <double> offsetVector = new DenseVector(3);
foreach (var normal in distinctNormals)
{
offsetVector = offsetVector + normal;
}
offsetVector = (offsetVector / distinctNormals.Length);
//for normal pointing to the outside *-1
offsetVectors[index] = offsetVector.Normalize(2) * (-1);
}
}
public static Vector <double> GradientProjection(Vector <double> x0, double eps)
{
var xCur = x0.GetProjectionHyperplane(HyperPlane, 1);
Vector <double> grad = new DenseVector(3)
{
[0] = Df_dx(xCur[0], xCur[1], xCur[2], eps),
[1] = Df_dy(xCur[0], xCur[1], xCur[2], eps),
[2] = Df_dz(xCur[0], xCur[1], xCur[2], eps)
};
//подбираем длину шага в направлении проекции
var alpha = GoldenRatioAlpha(xCur, eps, grad.Normalize(2).GetProjectionHyperplane(HyperPlane, 1)); //project???
//.GetProjectionHyperplane(HyperPlane, 1)
//(HyperPlane,a)=1
var xNext = (xCur - alpha * grad.Normalize(2)).GetProjectionHyperplane(HyperPlane, 1); //нормируем направление шага
var iteration = 1; // normalized
do
{
#region Output
if (iteration < 100000)
{
Console.WriteLine($"Iter {iteration++}: xCur:{(xCur).ToStR()}" + $" F: {F(xCur)}" +
$"||Grad||:{grad.L2Norm()} " +
$"||xCur-xNext||:{(xCur - xNext).L2Norm()}");
}
else
{
Console.WriteLine(alpha);
}
#endregion
if (!IsOnPlane(HyperPlane, xCur, eps))
{
throw new Exception();
}
xCur = xNext;
grad[0] = Df_dx(xCur[0], xCur[1], xCur[2], eps / 1000);
grad[1] = Df_dy(xCur[0], xCur[1], xCur[2], eps / 1000);
grad[2] = Df_dz(xCur[0], xCur[1], xCur[2], eps / 1000);
//подбираем длину шага в направлении проекции
alpha = GoldenRatioAlpha(xCur, eps, grad); //project???
xNext = (xCur - alpha * grad)
.GetProjectionHyperplane(HyperPlane, 1); //вводим нормированный градиент
}while (!((xCur - xNext).L2Norm() < eps)); //normalized now
Console.WriteLine($"Iter {iteration}: xCur:{xNext.ToStR()} " +
$"||xCur-xNext||:{(xCur - xNext).L2Norm()} " +
$"F = : {F(xNext)}");
return(xNext);
}
