private Movment GetMovment(Vector3D translation, AngleAxis angleAxis)
{
double X = GetValue(translation.X);
double Y = GetValue(translation.Z);
double angle = IK.CalculateIKOneJoint(X, Y);
int direction = 1;
if (translation.Z < 0)
{
angle = IK.Rotate180Degrees(angle);
direction = -1;
}
angle = Math.Abs(angle);
if(angle == 180)
{
angle = 0;
}
double speed = IK.lengthBetwenSolderandWrist(X, Y) / 1600;
var rotation = (angleAxis.Y * angleAxis.Angle) / 1600;
var centerX = (angleAxis.Z * angleAxis.Angle) / 75;
var centerY = (angleAxis.X * angleAxis.Angle) / 75;
return new Movment(angle, direction, speed, rotation, centerX, centerY);
}
public void AngleAxisAxis()
{
tlog.Debug(tag, $"AngleAxisAxis START");
using (Radian radian = new Radian(0.3f))
{
using (Vector3 vector = new Vector3(1.0f, 2.0f, 3.0f))
{
var testingTarget = new AngleAxis(radian, vector);
Assert.IsNotNull(testingTarget, "Can't create success object AngleAxis");
Assert.IsInstanceOf <AngleAxis>(testingTarget, "Should be an instance of AngleAxis type.");
Assert.AreEqual(1.0f, testingTarget.axis.X, "Should be equal!");
Assert.AreEqual(2.0f, testingTarget.axis.Y, "Should be equal!");
Assert.AreEqual(3.0f, testingTarget.axis.Z, "Should be equal!");
testingTarget.axis = new Vector3(3.0f, 2.0f, 1.0f);
Assert.AreEqual(3.0f, testingTarget.axis.X, "Should be equal!");
Assert.AreEqual(2.0f, testingTarget.axis.Y, "Should be equal!");
Assert.AreEqual(1.0f, testingTarget.axis.Z, "Should be equal!");
testingTarget.Dispose();
}
}
tlog.Debug(tag, $"AngleAxisAxis END (OK)");
}
public void AngleAxis()
{
var s1 = new Axes.AngleAxis();
var s2 = new AngleAxis();
OxyAssert.PropertiesAreEqual(s1, s2);
}
/// <summary>
/// Rotates the cue around the axis along which it is aiming.
/// </summary>
/// <param name="angle">Angle in degrees.</param>
public void RotateAroundCueAxis(float angle)
{
Vector3 axis = Vector3.forward;
var q = new AngleAxis(angle, axis).Quaternion;
transform.localRotation *= q;
}
public AngleAxis AngleAxis()
{
if (angleAxis == null)
{
angleAxis = new AngleAxis();
}
return(angleAxis);
}
public AngleAxis GetRotationAxis()
{
AngleAxis rotation = new AngleAxis();
rotation.X = MouseInfos.RotationX;
rotation.Y = MouseInfos.RotationY;
rotation.Z = MouseInfos.RotationZ;
rotation.Angle = MouseInfos.Angle;
return(rotation);
}
public void AngleAxisConstructor()
{
tlog.Debug(tag, $"AngleAxisConstructor START");
var testingTarget = new AngleAxis();
Assert.IsNotNull(testingTarget, "Can't create success object AngleAxis");
Assert.IsInstanceOf <AngleAxis>(testingTarget, "Should be an instance of AngleAxis type.");
testingTarget.Dispose();
tlog.Debug(tag, $"AngleAxisConstructor END (OK)");
}
public static PlotModel OffsetAngles()
{
var model = new PlotModel
{
Title = "Offset angle axis",
PlotType = PlotType.Polar,
PlotAreaBorderThickness = new OxyThickness(0),
PlotMargins = new OxyThickness(60, 20, 4, 40)
};
var angleAxis = new AngleAxis
{
Minimum = 0,
Maximum = Math.PI * 2,
MajorStep = Math.PI / 4,
MinorStep = Math.PI / 16,
StringFormat = "0.00",
StartAngle = 30,
EndAngle = 390
};
model.Axes.Add(angleAxis);
model.Axes.Add(new MagnitudeAxis());
model.Series.Add(new FunctionSeries(t => t, t => t, 0, Math.PI * 6, 0.01));
// Subscribe to the mouse down event on the line series.
model.MouseDown += (s, e) =>
{
var increment = 0d;
// Increment and decrement must be in degrees (corresponds to the StartAngle and EndAngle properties).
if (e.ChangedButton == OxyMouseButton.Left)
{
increment = 15;
}
if (e.ChangedButton == OxyMouseButton.Right)
{
increment = -15;
}
if (Math.Abs(increment) > double.Epsilon)
{
angleAxis.StartAngle += increment;
angleAxis.EndAngle += increment;
model.InvalidatePlot(false);
e.Handled = true;
}
};
return(model);
}
public void NDalicEqualTo()
{
tlog.Debug(tag, $"NDalicEqualTo START");
AngleAxis lhs = new AngleAxis();
AngleAxis rhs = new AngleAxis();
var result = NDalic.EqualTo(lhs, rhs);
tlog.Debug(tag, "EqualTo : " + result);
tlog.Debug(tag, $"NDalicEqualTo END (OK)");
}
public void PropertyValueConstructorWithAngleAxis()
{
tlog.Debug(tag, $"PropertyValueConstructorWithAngleAxis START");
using (AngleAxis angleAxis = new AngleAxis())
{
var testingTarget = new PropertyValue(angleAxis);
Assert.IsNotNull(testingTarget, "Should be not null!");
Assert.IsInstanceOf <PropertyValue>(testingTarget, "Should return PropertyValue instance.");
testingTarget.Dispose();
}
tlog.Debug(tag, $"PropertyValueConstructorWithAngleAxis END (OK)");
}
public void AngleAxisConstructorWithRadianAndVector3()
{
tlog.Debug(tag, $"AngleAxisConstructorWithRadianAndVector3 START");
using (Radian radian = new Radian(0.3f))
{
using (Vector3 vector = new Vector3(1.0f, 2.0f, 3.0f))
{
var testingTarget = new AngleAxis(radian, vector);
Assert.IsNotNull(testingTarget, "Can't create success object AngleAxis");
Assert.IsInstanceOf <AngleAxis>(testingTarget, "Should be an instance of AngleAxis type.");
testingTarget.Dispose();
}
}
tlog.Debug(tag, $"AngleAxisConstructorWithRadianAndVector3 END (OK)");
}
/// <summary>
/// Helper method that helps to compute the difference in translation and rotation of an object moving from one frame to the other
/// This is essentially used for motion blur
/// </summary>
/// <param name="_Previous">The object's matrix at previous frame</param>
/// <param name="_Current">The object's matrix at current frame</param>
/// <param name="_DeltaPosition">Returns the difference in position from last frame</param>
/// <param name="_DeltaRotation">Returns the difference in rotation from last frame</param>
/// <param name="_Pivot">Returns the pivot position the object rotated about</param>
public static void ComputeObjectDeltaPositionRotation(ref Matrix4x4 _Previous, ref Matrix4x4 _Current, out Vector _DeltaPosition, out Quat _DeltaRotation, out Vector _Pivot)
{
// Compute the rotation the matrix sustained
Quat PreviousRotation = QuatFromMatrix(_Previous);
Quat CurrentRotation = QuatFromMatrix(_Current);
_DeltaRotation = QuatMultiply(QuatInvert(PreviousRotation), CurrentRotation);
Vector PreviousPosition = (Vector)_Previous.GetRow3();
Vector CurrentPosition = (Vector)_Current.GetRow3();
// Retrieve the pivot point about which that rotation occurred
_Pivot = CurrentPosition;
AngleAxis AA = new AngleAxis(_DeltaRotation);
float RotationAngle = AA.Angle;
if (Math.Abs(RotationAngle) > 1e-4f)
{
Vector RotationAxis = AA.Axis;
Vector Previous2Current = CurrentPosition - PreviousPosition;
float L = Previous2Current.Length;
if (L > 1e-4f)
{
Previous2Current /= L;
Vector N = Previous2Current.Cross(RotationAxis);
N.Normalize();
Vector MiddlePoint = 0.5f * (PreviousPosition + CurrentPosition);
float Distance2Pivot = 0.5f * L / (float)Math.Tan(0.5f * RotationAngle);
_Pivot = MiddlePoint + N * Distance2Pivot;
}
// Rotate previous position about pivot, this should yield us current position
Vector RotatedPreviousPosition = RotateAbout(PreviousPosition, _Pivot, _DeltaRotation);
// // Update previous position so the remaining position gap is filled by delta translation
// PreviousPosition = RotatedPreviousPosition;
PreviousPosition = CurrentPosition; // Close the gap so we have no delta translation
}
_DeltaPosition = CurrentPosition - PreviousPosition; // Easy !
}
public void AngleAxisGetCPtr()
{
tlog.Debug(tag, $"AngleAxisGetCPtr START");
var testingTarget = new AngleAxis();
Assert.IsNotNull(testingTarget, "Can't create success object AngleAxis");
Assert.IsInstanceOf <AngleAxis>(testingTarget, "Should be an instance of AngleAxis type.");
try
{
AngleAxis.getCPtr(testingTarget);
}
catch (Exception e)
{
tlog.Debug(tag, e.Message.ToString());
Assert.Fail("Caught Exception : Failed!");
}
testingTarget.Dispose();
tlog.Debug(tag, $"AngleAxisGetCPtr END (OK)");
}
public void drawRotation()
{
this.rotationState = this.rotationPicker.DrawGUI(this.rotationState);
this.toView.localRotation = Quaternion.AngleAxis(this.rotationState.angle,this.rotationState.axis);
}
static void ClockExample()
{
var plot = new PlotModel();
plot.PlotType = PlotType.Polar;
plot.PlotAreaBorderThickness = new OxyThickness(0.0);
var mag = new MagnitudeAxis();
mag.Minimum = 0.0;
mag.Maximum = 1.0;
mag.MinimumPadding = 0.0;
mag.MaximumPadding = 0.0;
mag.IsAxisVisible = false;
plot.Axes.Add(mag);
var ang = new AngleAxis();
ang.Minimum = 0.0;
ang.Maximum = 360.0;
ang.MajorStep = 90.0;
ang.MinorStep = 30.0;
ang.StartAngle = 90.0;
ang.EndAngle = -270.0;
ang.LabelFormatter = angle => angle == 0 ? "12" : (angle / 30).ToString();
plot.Axes.Add(ang);
var hour = new LineSeries();
hour.StrokeThickness = 6.0;
plot.Series.Add(hour);
var minute = new LineSeries();
minute.StrokeThickness = 4.0;
plot.Series.Add(minute);
var second = new LineSeries();
second.StrokeThickness = 2.0;
plot.Series.Add(second);
void update()
{
var now = DateTime.Now;
plot.Title = now.ToLongDateString();
plot.Subtitle = now.ToLongTimeString();
hour.Points.Clear();
hour.Points.Add(new DataPoint(0.0, 30 * (now.TimeOfDay.TotalHours % 12.0)));
hour.Points.Add(new DataPoint(0.4, 30 * (now.TimeOfDay.TotalHours % 12.0)));
minute.Points.Clear();
minute.Points.Add(new DataPoint(0.0, 6 * (now.TimeOfDay.TotalMinutes % 60)));
minute.Points.Add(new DataPoint(0.6, 6 * (now.TimeOfDay.TotalMinutes % 60)));
second.Points.Clear();
second.Points.Add(new DataPoint(-0.2, 6 * now.Second));
second.Points.Add(new DataPoint(0.8, 6 * now.Second));
plot.InvalidatePlot(true);
}
OxySnel.Snel.Show(plot);
Task.Run(async() =>
{
while (true)
{
await Task.Delay(100);
await Snel.Invoke(update);
}
});
Console.ReadKey(true);
Snel.Kill();
}
public void AngleAxis()
{
var s1 = new OxyPlot.Axes.AngleAxis();
var s2 = new AngleAxis();
OxyAssert.PropertiesAreEqual(s1, s2);
}
public AngleAxis AngleAxis(AngleAxis angleAxis)
{
this.angleAxis = angleAxis;
return(this.angleAxis);
}
/// <summary>
/// Helper method that helps to compute the difference in translation and rotation of an object moving from one frame to the other
/// This is essentially used for motion blur
/// </summary>
/// <param name="_Previous">The object's matrix at previous frame</param>
/// <param name="_Current">The object's matrix at current frame</param>
/// <param name="_DeltaPosition">Returns the difference in position from last frame</param>
/// <param name="_DeltaRotation">Returns the difference in rotation from last frame</param>
/// <param name="_Pivot">Returns the pivot position the object rotated about</param>
public static void ComputeObjectDeltaPositionRotation( ref Matrix4x4 _Previous, ref Matrix4x4 _Current, out Vector _DeltaPosition, out Quat _DeltaRotation, out Vector _Pivot )
{
// Compute the rotation the matrix sustained
Quat PreviousRotation = QuatFromMatrix( _Previous );
Quat CurrentRotation = QuatFromMatrix( _Current );
_DeltaRotation = QuatMultiply( QuatInvert( PreviousRotation ), CurrentRotation );
Vector PreviousPosition = (Vector) _Previous.GetRow3();
Vector CurrentPosition = (Vector) _Current.GetRow3();
// Retrieve the pivot point about which that rotation occurred
_Pivot = CurrentPosition;
AngleAxis AA = new AngleAxis( _DeltaRotation );
float RotationAngle = AA.Angle;
if ( Math.Abs( RotationAngle ) > 1e-4f )
{
Vector RotationAxis = AA.Axis;
Vector Previous2Current = CurrentPosition - PreviousPosition;
float L = Previous2Current.Length;
if ( L > 1e-4f )
{
Previous2Current /= L;
Vector N = Previous2Current.Cross( RotationAxis );
N.Normalize();
Vector MiddlePoint = 0.5f * (PreviousPosition + CurrentPosition);
float Distance2Pivot = 0.5f * L / (float) Math.Tan( 0.5f * RotationAngle );
_Pivot = MiddlePoint + N * Distance2Pivot;
}
// Rotate previous position about pivot, this should yield us current position
Vector RotatedPreviousPosition = RotateAbout( PreviousPosition, _Pivot, _DeltaRotation );
// // Update previous position so the remaining position gap is filled by delta translation
// PreviousPosition = RotatedPreviousPosition;
PreviousPosition = CurrentPosition; // Close the gap so we have no delta translation
}
_DeltaPosition = CurrentPosition - PreviousPosition; // Easy !
}
internal static global::System.Runtime.InteropServices.HandleRef getCPtr(AngleAxis obj)
{
return((obj == null) ? new global::System.Runtime.InteropServices.HandleRef(null, global::System.IntPtr.Zero) : obj.swigCPtr);
}
public void drawRotation()
{
this.rotationState = this.rotationPicker.DrawGUI(this.rotationState);
this.toView.localRotation = Quaternion.AngleAxis(this.rotationState.angle, this.rotationState.axis);
}
