static void rotateAroundAxis(DoubleTransform dtr, DVec3 localAxis, double angle)
{
var axis = (dtr.localRotation * localAxis).normalized;
var rot = DQuat.fromAxisAngle(axis, angle).normalized;
dtr.localRotation = (rot * dtr.localRotation).normalized;
}
public static DQuat slerp(DQuat a, DQuat b, double t)
{
double dotVal = dot(a, b);
double angle = Math.Acos(dotVal);
return((a * Math.Sin(angle * (1.0 - t)) + b * Math.Sin(angle * t)) / Math.Sin(angle));
}
public static DVec3 operator*(DQuat q, DVec3 v)
{
var qv = new DQuat(v.x, v.y, v.z, 0.0);
DQuat result = q * qv * q.conjugate;
return(new DVec3(result.x, result.y, result.z));
}
//DEBUG
void DrawRotation_Trail()
{
int Length = (Quat.Count < TrailLength) ? Quat.Count : TrailLength;
LineRenderer Quat_Trail_Renderer = Quat_Trail.GetComponent <LineRenderer>();
Quat_Trail_Renderer.positionCount = Length - 1;
for (int i = 0; i < Length - 1; i++)
{ //Debug
Quaternion DQuat;
Vector3 QuatAxis;
float QuatAngle;
if (Quat.Count < 2)
{
DQuat = Quaternion.identity;
}
else
{
DQuat = Quaternion.Inverse(Quat[Quat.Count - 2 - i]) * Quat[Quat.Count - 1 - i];
}
DQuat.ToAngleAxis(out QuatAngle, out QuatAxis);
Vector3 TQuatAxis = RefMatInv * (Pos[Pos.Count - 1 - i] + QuatAxis.normalized * QuatAngle * GizmoQuatScale);
Quat_Trail_Renderer.SetPosition(i, TQuatAxis + RefPos);
}
Quat_Trail_Renderer.startWidth = 0.01f * TrailLinesWidth;
Quat_Trail_Renderer.endWidth = TrailLinesWidth;
}
public static DQuat lerp(DQuat a, DQuat b, double t)
{
return(new DQuat(
(b.x - a.x) * t + a.x,
(b.y - a.y) * t + a.y,
(b.z - a.z) * t + a.z,
(b.w - a.w) * t + a.w
).normalized);
}
public override bool Equals(object other)
{
if (!(other is DQuat))
{
return(false);
}
DQuat v = (DQuat)other;
return(v.x.Equals(x) && v.y.Equals(y) && v.z.Equals(z) && v.w.Equals(w));
}
void setRotation(DQuat rotation)
{
localRotation = rotation * getParentRotation().conjugate;
}
public static double dot(DQuat a, DQuat b)
{
return((a.x * b.x) + (a.y * b.y) + (a.z * b.z) + (a.w * b.w));
}
void Update()
{
if (Record)
{
RecordData();
}
// Draw Vector
if (Vector)
{
if (SpeedActive)
{
Speed_Vect.SetActive(true);
DrawVector(Speed_Vect, Speed[Speed.Count - 1], GizmoSpeedScale);
}
else
{
Speed_Vect.SetActive(false);
}
if (AccActive)
{
Acc_Vect.SetActive(true);
DrawVector(Acc_Vect, Acc[Acc.Count - 1], GizmoAccScale);
}
else
{
Acc_Vect.SetActive(false);
}
if (JerkActive)
{
Jerk_Vect.SetActive(true);
DrawVector(Jerk_Vect, Jerk[Jerk.Count - 1], GizmoJerkScale);
}
else
{
Jerk_Vect.SetActive(false);
}
if (OrientationActive)
{
Vector3 QuatAxis;
float QuatAngle;
Quaternion DQuat;
if (Quat.Count < 2)
{
DQuat = Quaternion.identity;
}
else
{
DQuat = Quaternion.Inverse(Quat[Quat.Count - 2]) * Quat[Quat.Count - 1];
}
DQuat.ToAngleAxis(out QuatAngle, out QuatAxis);
Quat_Vect.SetActive(true);
DrawVector(Quat_Vect, QuatAxis * QuatAngle, GizmoQuatScale);
}
else
{
Quat_Vect.SetActive(false);
}
}
else
{
Speed_Vect.SetActive(false);
Acc_Vect.SetActive(false);
Jerk_Vect.SetActive(false);
Quat_Vect.SetActive(false);
}
//Draw Trail
if (Trail)
{
//Draw Trails and vect
if (PosActive)
{
Pos_Trail.SetActive(true);
Draw_Trail(Pos, Pos_Trail, 1);
}
else
{
Pos_Trail.SetActive(false);
}
if (SpeedActive)
{
Speed_Ribbon.SetActive(true);
Draw_Ribbon(Speed, Pos, Speed_Ribbon, GizmoSpeedScale);
}
else
{
Speed_Ribbon.SetActive(false);
}
if (AccActive)
{
Acc_Ribbon.SetActive(true);
Draw_Ribbon(Acc, Pos, Acc_Ribbon, GizmoAccScale);
}
else
{
Acc_Ribbon.SetActive(false);
}
if (JerkActive)
{
Jerk_Ribbon.SetActive(true);
Draw_Ribbon(Jerk, Pos, Jerk_Ribbon, GizmoJerkScale);
}
else
{
Jerk_Ribbon.SetActive(false);
}
if (OrientationActive)
{
Quat_Trail.SetActive(true);
}
else
{
Quat_Trail.SetActive(false);
}
}
else
{
Pos_Trail.SetActive(false);
Speed_Ribbon.SetActive(false);
Acc_Ribbon.SetActive(false);
Jerk_Ribbon.SetActive(false);
Quat_Trail.SetActive(false);
}
}