public static System.Numerics.Vector3[] SpherePointCloud(float radius, System.Numerics.Vector3 center, int pointsNum)
{
var data = new System.Numerics.Vector3[pointsNum];
double x, yPos, y, zPos, z;
for (int i = 0; i < pointsNum; i++)
{
x = radius * (2 * RandomThreadStatic.NextDouble() - 1);
yPos = Math.Sqrt(radius * radius - x * x);
y = yPos * (2 * RandomThreadStatic.NextDouble() - 1);
zPos = Math.Sqrt(yPos * yPos - y * y);
z = RandomThreadStatic.Next(0, 2) == 0 ? -zPos : zPos;
data[i] = new System.Numerics.Vector3((float)x, (float)y, (float)z) + center;
}
return(data);
}
private BezierCurve Repro(BezierCurve bezierCurve)
{
int pointIndex = RandomThreadStatic.Next(1, _bezierControlPointsCount + 1);
Vector3 direction = RandomThreadStatic.NextPointCube(1);
var bezierCurveRepro = new BezierCurve(bezierCurve.Points);
var bezierDirectionRepro = Vector3.Normalize(direction) * 1f * (float)RandomThreadStatic.NextDouble();
bezierCurveRepro.Points[pointIndex] += bezierDirectionRepro;
if (_manipulator.ApproxWithinReach(bezierCurveRepro.Points[pointIndex] + bezierDirectionRepro))
{
bezierCurveRepro.Points[pointIndex] += bezierDirectionRepro;
}
else
{
bezierCurveRepro.Points[pointIndex] -= bezierDirectionRepro;
}
return(bezierCurveRepro);
}
public override InverseKinematicsResult Execute(Manipulator manipulator, Vector3 goal, int joint = -1) // TODO: refactor
{
// use gripper if default joint
if (joint == -1)
{
joint = manipulator.Joints.Length - 1;
}
VectorFloat configuration = manipulator.q, dq = VectorFloat.Build.Dense(manipulator.Joints.Length);
var errorPos = goal - manipulator.Joints[joint].Position;
var error = VectorFloat.Build.Dense(new float[] { errorPos.X, errorPos.Y, errorPos.Z, 0, 0, 0 });
float distance = errorPos.Length();
float maxStepSize = _maxStepSize, scale = 1;
int iterations = 0;
while (distance > _threshold && iterations < _maxIterations)
{
errorMod.Add(error.L2Norm());
configs.Add(VectorFloat.Build.DenseOfVector(configuration));
float range, stepNeg, stepPos;
for (int i = 0; i <= joint; i++)
{
// checking coordinate constraints
range = manipulator.Joints[i].CoordinateRange.X - configuration[i] * MathUtil.SIMD_DEGS_PER_RAD;
stepNeg = range <= -maxStepSize ? -maxStepSize : range;
range = manipulator.Joints[i].CoordinateRange.Y - configuration[i] * MathUtil.SIMD_DEGS_PER_RAD;
stepPos = range >= maxStepSize ? maxStepSize : range;
// generating random coordinate offset
dq[i] = (stepNeg + (float)RandomThreadStatic.NextDouble() * (stepPos - stepNeg)) * MathUtil.SIMD_RADS_PER_DEG;
}
// get the new configuration
var configurationNew = VectorFloat.Build.DenseOfVector(configuration);
configurationNew.AddSubVector(dq);
var fkRes = manipulator.ForwardKinematics(configurationNew);
float distanceNew = fkRes.JointPositions[joint].DistanceTo(goal);
if (distanceNew < distance)
{
// update configuration, distance and scale
configuration = configurationNew;
errorPos = goal - fkRes.JointPositions[joint];
error = VectorFloat.Build.Dense(new float[] { errorPos.X, errorPos.Y, errorPos.Z, 0, 0, 0 });
scale *= distanceNew / distance;
distance = distanceNew;
maxStepSize = _maxStepSize * scale;
}
iterations++;
}
return(new InverseKinematicsResult
{
Converged = iterations < _maxIterations && !JointLimitsExceeded(manipulator, configuration),
Iterations = iterations,
Configuration = configuration,
Error = error
});
}