}/// <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(sfrotation 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>
}/// <summary>
/// copy constructor
/// </summary>
/// <param name="in1"></param>
public sfrotation(sfrotation 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>
/// 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(sfrotation 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;
}
/// copy constructor
/// </summary>
/// <param name="in1"></param>
public sfrotation(sfrotation 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;
}