/// <summary>
/// copy constructor
/// </summary>
/// <param name="in1"></param>
public classRotation(classRotation in1)
{
x = (in1 != null) ? in1.x : 0;
y = (in1 != null) ? in1.y : 0;
z = (in1 != null) ? in1.z : 1;
angle = (in1 != null) ? in1.angle : 0;
coding = (in1 != null) ? in1.coding : (int)cde.CODING_AXISANGLE;
}
/// <summary>
/// calcultes new translation when combining translations
///
/// Rt = Ra Rb
/// Ct = Cb
/// Tt = Ra (Cb + Tb - Ca) + Ca + Ta - Cb
///
/// for theory:
/// http://www.euclideanspace.com/maths/geometry/rotations/rotationAndTranslation/nonMatrix/index.htm
/// </summary>
/// <param name="ta">Ta = translation of transform a in absolute coordinates</param>
/// <param name="ra">Ra = rotation function of transform a in absolute coordinates</param>
/// <param name="ca">Ca = centre of rotation of transform a in absolute coordinates</param>
/// <param name="tb">Tb = translation of transform b in coordinates of transform a</param>
/// <param name="cb">Cb = centre of rotation of transform b in coordinates of transform a</param>
/// <returns>Tt total offset</returns>
public vector3D rotationOffset(vector3D ta, classRotation ra, vector3D ca, vector3D tb, vector3D cb)
{
vector3D result = new vector3D(cb);
result.add(tb);
result.sub(ca);
if (ra != null)
{
ra.transform(result);
}
result.add(ca);
result.add(ta);
result.sub(cb);
return(result);
}
/// <summary>
/// calculate total rotation by taking current rotation and then
/// apply rotation r
///
/// if both angles are quaternions then this is a multiplication
/// </summary>
/// <param name="r"></param>
public void combine(classRotation r)
{
toQuaternion();
if (r == null)
{
return;
}
double qax = x;
double qay = y;
double qaz = z;
double qaw = angle;
double qbx;
double qby;
double qbz;
double qbw;
if (r.coding == (int)cde.CODING_QUATERNION)
{
qbx = r.x;
qby = r.y;
qbz = r.z;
qbw = r.angle;
}
else
{
double s = Math.Sin(r.angle / 2);
qbx = r.x * s;
qby = r.y * s;
qbz = r.z * s;
qbw = Math.Cos(r.angle / 2);
}
// now multiply the quaternions
angle = qaw * qbw - qax * qbx - qay * qby - qaz * qbz;
x = qax * qbw + qaw * qbx + qay * qbz - qaz * qby;
y = qaw * qby - qax * qbz + qay * qbw + qaz * qbx;
z = qaw * qbz + qax * qby - qay * qbx + qaz * qbw;
coding = (int)cde.CODING_QUATERNION;
}
/// <summary>
/// calculate total rotation by taking current rotation and then
/// apply rotation r
///
/// if both angles are quaternions then this is a multiplication
/// </summary>
/// <param name="r"></param>
public void combine(classRotation r)
{
toQuaternion();
if (r == null) return;
double qax = x;
double qay = y;
double qaz = z;
double qaw = angle;
double qbx;
double qby;
double qbz;
double qbw;
if (r.coding == (int)cde.CODING_QUATERNION)
{
qbx = r.x;
qby = r.y;
qbz = r.z;
qbw = r.angle;
}
else
{
double s = Math.Sin(r.angle / 2);
qbx = r.x * s;
qby = r.y * s;
qbz = r.z * s;
qbw = Math.Cos(r.angle / 2);
}
// now multiply the quaternions
angle = qaw * qbw - qax * qbx - qay * qby - qaz * qbz;
x = qax * qbw + qaw * qbx + qay * qbz - qaz * qby;
y = qaw * qby - qax * qbz + qay * qbw + qaz * qbx;
z = qaw * qbz + qax * qby - qay * qbx + qaz * qbw;
coding = (int)cde.CODING_QUATERNION;
}
/// <summary>
/// copy constructor
/// </summary>
/// <param name="in1"></param>
public classRotation(classRotation in1)
{
x = (in1 != null) ? in1.x : 0;
y = (in1 != null) ? in1.y : 0;
z = (in1 != null) ? in1.z : 1;
angle = (in1 != null) ? in1.angle : 0;
coding = (in1 != null) ? in1.coding : (int)cde.CODING_AXISANGLE;
}
/// <summary>
/// calcultes new translation when combining translations
///
/// Rt = Ra Rb
/// Ct = Cb
/// Tt = Ra (Cb + Tb - Ca) + Ca + Ta - Cb
///
/// for theory:
/// http://www.euclideanspace.com/maths/geometry/rotations/rotationAndTranslation/nonMatrix/index.htm
/// </summary>
/// <param name="ta">Ta = translation of transform a in absolute coordinates</param>
/// <param name="ra">Ra = rotation function of transform a in absolute coordinates</param>
/// <param name="ca">Ca = centre of rotation of transform a in absolute coordinates</param>
/// <param name="tb">Tb = translation of transform b in coordinates of transform a</param>
/// <param name="cb">Cb = centre of rotation of transform b in coordinates of transform a</param>
/// <returns>Tt total offset</returns>
public vector3D rotationOffset(vector3D ta, classRotation ra, vector3D ca, vector3D tb, vector3D cb)
{
vector3D result = new vector3D(cb);
result.add(tb);
result.sub(ca);
if (ra != null) ra.transform(result);
result.add(ca);
result.add(ta);
result.sub(cb);
return result;
}
