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); }