//Mouse drag, calculate rotation
public void drag(Point NewPt, Quat4f NewRot)
{
//Map the point to the sphere
this.mapToSphere(NewPt, EnVec);
//Return the quaternion equivalent to the rotation
if (NewRot != null)
{
Vector3f Perp = new Vector3f();
//Compute the vector perpendicular to the begin and end vectors
Vector3f.cross(Perp, StVec, EnVec);
//Compute the length of the perpendicular vector
if (Perp.length() > Epsilon) //if its non-zero
{
//We're ok, so return the perpendicular vector as the transform after all
NewRot.x = Perp.x;
NewRot.y = Perp.y;
NewRot.z = Perp.z;
//In the quaternion values, w is cosine (theta / 2), where theta is rotation angle
NewRot.w = Vector3f.dot(StVec, EnVec);
}
else //if its zero
{
//The begin and end vectors coincide, so return an identity transform
NewRot.x = NewRot.y = NewRot.z = NewRot.w = 0.0f;
}
}
}
public void SetTranslationAndRotation( Vector3f translation, Quat4f rotation )
{
this.t_ = translation;
this.q_ = rotation;
// TODO:
// emit modified
}
public override EntityRef create(Prefab prefab, Vector3f position, Quat4f rotation)
{
IList <Component> components = Lists.newArrayList();
foreach (Component component in prefab.iterateComponents())
{
Component newComp = componentLibrary.copy(component);
components.Add(newComp);
if (newComp is LocationComponent)
{
LocationComponent loc = (LocationComponent)newComp;
loc.WorldPosition = position;
loc.WorldRotation = rotation;
}
}
components.Add(new EntityInfoComponent(prefab.Name, prefab.Persisted, prefab.AlwaysRelevant));
return(create(components));
}
/// <summary>
/// Sets the rotational component (upper 3x3) of this matrix to the
/// matrix equivalent values of the quaternion argument; the other
/// elements of this matrix are unchanged; a singular value
/// decomposition is performed on this object's upper 3x3 matrix to
/// factor out the scale, then this object's upper 3x3 matrix components
/// are replaced by the matrix equivalent of the quaternion,
/// and then the scale is reapplied to the rotational components.
/// </summary>
/// <remarks>
/// Sets the rotational component (upper 3x3) of this matrix to the
/// matrix equivalent values of the quaternion argument; the other
/// elements of this matrix are unchanged; a singular value
/// decomposition is performed on this object's upper 3x3 matrix to
/// factor out the scale, then this object's upper 3x3 matrix components
/// are replaced by the matrix equivalent of the quaternion,
/// and then the scale is reapplied to the rotational components.
/// </remarks>
/// <param name="q1">the quaternion that specifies the rotation</param>
public void SetRotation(Quat4f q1)
{
double[] tmp_rot = new double[9];
// scratch matrix
double[] tmp_scale = new double[3];
// scratch matrix
GetScaleRotate(tmp_scale, tmp_rot);
m00 = (float)((1.0f - 2.0f * q1.y * q1.y - 2.0f * q1.z * q1.z) * tmp_scale[0]);
m10 = (float)((2.0f * (q1.x * q1.y + q1.w * q1.z)) * tmp_scale[0]);
m20 = (float)((2.0f * (q1.x * q1.z - q1.w * q1.y)) * tmp_scale[0]);
m01 = (float)((2.0f * (q1.x * q1.y - q1.w * q1.z)) * tmp_scale[1]);
m11 = (float)((1.0f - 2.0f * q1.x * q1.x - 2.0f * q1.z * q1.z) * tmp_scale[1]);
m21 = (float)((2.0f * (q1.y * q1.z + q1.w * q1.x)) * tmp_scale[1]);
m02 = (float)((2.0f * (q1.x * q1.z + q1.w * q1.y)) * tmp_scale[2]);
m12 = (float)((2.0f * (q1.y * q1.z - q1.w * q1.x)) * tmp_scale[2]);
m22 = (float)((1.0f - 2.0f * q1.x * q1.x - 2.0f * q1.y * q1.y) * tmp_scale[2]);
}
/// <summary>
/// Sets the value of this matrix to the matrix conversion of the
/// single precision quaternion argument.
/// </summary>
/// <remarks>
/// Sets the value of this matrix to the matrix conversion of the
/// single precision quaternion argument.
/// </remarks>
/// <param name="q1">the quaternion to be converted</param>
public void Set(Quat4f q1)
{
this.m00 = (1.0f - 2.0f * q1.y * q1.y - 2.0f * q1.z * q1.z);
this.m10 = (2.0f * (q1.x * q1.y + q1.w * q1.z));
this.m20 = (2.0f * (q1.x * q1.z - q1.w * q1.y));
this.m01 = (2.0f * (q1.x * q1.y - q1.w * q1.z));
this.m11 = (1.0f - 2.0f * q1.x * q1.x - 2.0f * q1.z * q1.z);
this.m21 = (2.0f * (q1.y * q1.z + q1.w * q1.x));
this.m02 = (2.0f * (q1.x * q1.z + q1.w * q1.y));
this.m12 = (2.0f * (q1.y * q1.z - q1.w * q1.x));
this.m22 = (1.0f - 2.0f * q1.x * q1.x - 2.0f * q1.y * q1.y);
this.m03 = (float)0.0;
this.m13 = (float)0.0;
this.m23 = (float)0.0;
this.m30 = (float)0.0;
this.m31 = (float)0.0;
this.m32 = (float)0.0;
this.m33 = (float)1.0;
}
/// <summary>
/// Sets the value of this matrix from the rotation expressed
/// by the quaternion q1, the translation t1, and the scale s.
/// </summary>
/// <remarks>
/// Sets the value of this matrix from the rotation expressed
/// by the quaternion q1, the translation t1, and the scale s.
/// </remarks>
/// <param name="q1">the rotation expressed as a quaternion</param>
/// <param name="t1">the translation</param>
/// <param name="s">the scale value</param>
public void Set(Quat4f q1, Vector3f t1, float s)
{
this.m00 = (s * (1.0f - 2.0f * q1.y * q1.y - 2.0f * q1.z * q1.z));
this.m10 = (s * (2.0f * (q1.x * q1.y + q1.w * q1.z)));
this.m20 = (s * (2.0f * (q1.x * q1.z - q1.w * q1.y)));
this.m01 = (s * (2.0f * (q1.x * q1.y - q1.w * q1.z)));
this.m11 = (s * (1.0f - 2.0f * q1.x * q1.x - 2.0f * q1.z * q1.z));
this.m21 = (s * (2.0f * (q1.y * q1.z + q1.w * q1.x)));
this.m02 = (s * (2.0f * (q1.x * q1.z + q1.w * q1.y)));
this.m12 = (s * (2.0f * (q1.y * q1.z - q1.w * q1.x)));
this.m22 = (s * (1.0f - 2.0f * q1.x * q1.x - 2.0f * q1.y * q1.y));
this.m03 = t1.x;
this.m13 = t1.y;
this.m23 = t1.z;
this.m30 = (float)0.0;
this.m31 = (float)0.0;
this.m32 = (float)0.0;
this.m33 = (float)1.0;
}
/// <summary>
/// Sets the value of this matrix to the matrix conversion of the
/// single precision quaternion argument.
/// </summary>
/// <remarks>
/// Sets the value of this matrix to the matrix conversion of the
/// single precision quaternion argument.
/// </remarks>
/// <param name="q1">the quaternion to be converted</param>
public void Set(Quat4f q1)
{
this.m00 = (1.0 - 2.0 * q1.y * q1.y - 2.0 * q1.z * q1.z);
this.m10 = (2.0 * (q1.x * q1.y + q1.w * q1.z));
this.m20 = (2.0 * (q1.x * q1.z - q1.w * q1.y));
this.m01 = (2.0 * (q1.x * q1.y - q1.w * q1.z));
this.m11 = (1.0 - 2.0 * q1.x * q1.x - 2.0 * q1.z * q1.z);
this.m21 = (2.0 * (q1.y * q1.z + q1.w * q1.x));
this.m02 = (2.0 * (q1.x * q1.z + q1.w * q1.y));
this.m12 = (2.0 * (q1.y * q1.z - q1.w * q1.x));
this.m22 = (1.0 - 2.0 * q1.x * q1.x - 2.0 * q1.y * q1.y);
}
/// <summary>
/// Constructs and initializes a Matrix4f from the quaternion,
/// translation, and scale values; the scale is applied only to the
/// rotational components of the matrix (upper 3x3) and not to the
/// translational components.
/// </summary>
/// <remarks>
/// Constructs and initializes a Matrix4f from the quaternion,
/// translation, and scale values; the scale is applied only to the
/// rotational components of the matrix (upper 3x3) and not to the
/// translational components.
/// </remarks>
/// <param name="q1">the quaternion value representing the rotational component</param>
/// <param name="t1">the translational component of the matrix</param>
/// <param name="s">the scale value applied to the rotational components</param>
public Matrix4f(Quat4f q1, Vector3f t1, float s)
{
m00 = (float)(s * (1.0 - 2.0 * q1.y * q1.y - 2.0 * q1.z * q1.z));
m10 = (float)(s * (2.0 * (q1.x * q1.y + q1.w * q1.z)));
m20 = (float)(s * (2.0 * (q1.x * q1.z - q1.w * q1.y)));
m01 = (float)(s * (2.0 * (q1.x * q1.y - q1.w * q1.z)));
m11 = (float)(s * (1.0 - 2.0 * q1.x * q1.x - 2.0 * q1.z * q1.z));
m21 = (float)(s * (2.0 * (q1.y * q1.z + q1.w * q1.x)));
m02 = (float)(s * (2.0 * (q1.x * q1.z + q1.w * q1.y)));
m12 = (float)(s * (2.0 * (q1.y * q1.z - q1.w * q1.x)));
m22 = (float)(s * (1.0 - 2.0 * q1.x * q1.x - 2.0 * q1.y * q1.y));
m03 = t1.x;
m13 = t1.y;
m23 = t1.z;
m30 = 0.0f;
m31 = 0.0f;
m32 = 0.0f;
m33 = 1.0f;
}
/// <summary>
/// Sets the orientation() of the Camera using polar coordinates.
/// \p theta rotates the Camera around its Y axis, and \e then \p phi rotates it around its X axis.
/// The polar coordinates are defined in the world coordinates system: \p theta = \p phi = 0 means
/// that the Camera is directed towards the world Z axis. Both angles are expressed in radians.
///
/// See also setUpVector(). The position() of the Camera is unchanged, you may want to call showEntireScene()
/// after this method to move the Camera.
///
/// This method can be useful to create Quicktime VR panoramic sequences, see the
/// QGLViewer::saveSnapshot() documentation for details. */
/// </summary>
/// <param name="theta"></param>
/// <param name="phi"></param>
public void SetOrientation( float theta, float phi )
{
var axis = new Vector3f( 0, 1, 0 );
var rot1 = new Quat4f( axis, theta );
axis = new Vector3f( ( float )( -Math.Cos( theta ) ), 0, ( float )( Math.Sin( theta ) ) );
var rot2 = new Quat4f( axis, phi );
Orientation = rot1 * rot2;
}
/// <summary>
/// Sets the value of this matrix from the rotation expressed
/// by the quaternion q1, the translation t1, and the scale s.
/// </summary>
/// <remarks>
/// Sets the value of this matrix from the rotation expressed
/// by the quaternion q1, the translation t1, and the scale s.
/// </remarks>
/// <param name="q1">the rotation expressed as a quaternion</param>
/// <param name="t1">the translation</param>
/// <param name="s">the scale value</param>
public void Set(Quat4f q1, Vector3f t1, float s)
{
this.m00 = s * (1.0 - 2.0 * q1.y * q1.y - 2.0 * q1.z * q1.z);
this.m10 = s * (2.0 * (q1.x * q1.y + q1.w * q1.z));
this.m20 = s * (2.0 * (q1.x * q1.z - q1.w * q1.y));
this.m01 = s * (2.0 * (q1.x * q1.y - q1.w * q1.z));
this.m11 = s * (1.0 - 2.0 * q1.x * q1.x - 2.0 * q1.z * q1.z);
this.m21 = s * (2.0 * (q1.y * q1.z + q1.w * q1.x));
this.m02 = s * (2.0 * (q1.x * q1.z + q1.w * q1.y));
this.m12 = s * (2.0 * (q1.y * q1.z - q1.w * q1.x));
this.m22 = s * (1.0 - 2.0 * q1.x * q1.x - 2.0 * q1.y * q1.y);
this.m03 = t1.x;
this.m13 = t1.y;
this.m23 = t1.z;
this.m30 = 0.0;
this.m31 = 0.0;
this.m32 = 0.0;
this.m33 = 1.0;
}
/// <summary>
/// Performs an SVD normalization of this matrix in order to acquire
/// the normalized rotational component; the values are placed into
/// the Quat4f parameter.
/// </summary>
/// <remarks>
/// Performs an SVD normalization of this matrix in order to acquire
/// the normalized rotational component; the values are placed into
/// the Quat4f parameter.
/// </remarks>
/// <param name="q1">quaternion into which the rotation component is placed</param>
public void Get(Quat4f q1)
{
double[] tmp_rot = new double[9];
// scratch matrix
double[] tmp_scale = new double[3];
// scratch matrix
GetScaleRotate(tmp_scale, tmp_rot);
double ww;
ww = 0.25 * (1.0 + tmp_rot[0] + tmp_rot[4] + tmp_rot[8]);
if (!((ww < 0 ? -ww : ww) < 1.0e-30))
{
q1.w = (float)Math.Sqrt(ww);
ww = 0.25 / q1.w;
q1.x = (float)((tmp_rot[7] - tmp_rot[5]) * ww);
q1.y = (float)((tmp_rot[2] - tmp_rot[6]) * ww);
q1.z = (float)((tmp_rot[3] - tmp_rot[1]) * ww);
return;
}
q1.w = 0.0f;
ww = -0.5 * (tmp_rot[4] + tmp_rot[8]);
if (!((ww < 0 ? -ww : ww) < 1.0e-30))
{
q1.x = (float)Math.Sqrt(ww);
ww = 0.5 / q1.x;
q1.y = (float)(tmp_rot[3] * ww);
q1.z = (float)(tmp_rot[6] * ww);
return;
}
q1.x = 0.0f;
ww = 0.5 * (1.0 - tmp_rot[8]);
if (!((ww < 0 ? -ww : ww) < 1.0e-30))
{
q1.y = (float)(Math.Sqrt(ww));
q1.z = (float)(tmp_rot[7] / (2.0 * q1.y));
return;
}
q1.y = 0.0f;
q1.z = 1.0f;
}
public void drag( Point MousePt )
{
Quat4f ThisQuat = new Quat4f();
arcBall.drag( MousePt, ThisQuat);
ThisRot.setRotation(ThisQuat);
ThisRot.mul( ThisRot, LastRot);
}
public Frame()
{
t_ = Vector3f.Zero;
q_ = Quat4f.Identity;
referenceFrame = null;
}
public void RotateAroundPoint( ref Quat4f rotation, Vector3f point )
{
// TODO: constrained
// if (constraint())
// constraint()->constrainRotation(rotation, this);
Rotation *= rotation;
Rotation = Rotation.Normalized(); // Prevents numerical drift
Quat4f q = new Quat4f( InverseTransformOf( rotation.Axis ), rotation.Angle );
Vector3f trans = point + q.Rotate( Position - point ) - Translation;
// if (constraint())
// constraint()->constrainTranslation(trans, this);
Translation += trans;
// emit modified();
}
public void Rotate( ref Quat4f q )
{
// if (constraint())
// constraint()->constrainRotation(q, this);
Rotation *= q;
Rotation = Rotation.Normalized();
// TODO: emit modified()
}
public void Rotate( Quat4f q )
{
Quat4f qbis = q;
Rotate( ref qbis );
}
public Frame( Vector3f translation, Quat4f rotation )
{
this.Translation = translation;
this.Rotation = rotation;
referenceFrame = null;
}
/// <summary>Constructs and initializes a Quat4d from the specified Quat4f.</summary>
/// <remarks>Constructs and initializes a Quat4d from the specified Quat4f.</remarks>
/// <param name="q1">the Quat4f containing the initialization x y z w data</param>
public Quat4d(Quat4f q1)
: base(q1)
{
}
public void setRotation(Quat4f q1)
{
float n, s;
float xs, ys, zs;
float wx, wy, wz;
float xx, xy, xz;
float yy, yz, zz;
n = (q1.x * q1.x) + (q1.y * q1.y) + (q1.z * q1.z) + (q1.w * q1.w);
s = (n > 0.0f) ? (2.0f / n) : 0.0f;
xs = q1.x * s;
ys = q1.y * s;
zs = q1.z * s;
wx = q1.w * xs;
wy = q1.w * ys;
wz = q1.w * zs;
xx = q1.x * xs;
xy = q1.x * ys;
xz = q1.x * zs;
yy = q1.y * ys;
yz = q1.y * zs;
zz = q1.z * zs;
M00 = 1.0f - (yy + zz);
M01 = xy - wz;
M02 = xz + wy;
M03 = 0f;
M10 = xy + wz;
M11 = 1.0f - (xx + zz);
M12 = yz - wx;
M13 = 0f;
M20 = xz - wy;
M21 = yz + wx;
M22 = 1.0f - (xx + yy);
M23 = 0f;
M30 = 0f;
M31 = 0f;
M32 = 0f;
M33 = 1f;
}
/// <summary>
/// Sets the value of this matrix to the matrix conversion of the
/// (single precision) quaternion argument.
/// </summary>
/// <remarks>
/// Sets the value of this matrix to the matrix conversion of the
/// (single precision) quaternion argument.
/// </remarks>
/// <param name="q1">the quaternion to be converted</param>
public void Set(Quat4f q1)
{
this.m00 = 1.0f - 2.0f * q1.y * q1.y - 2.0f * q1.z * q1.z;
this.m10 = 2.0f * (q1.x * q1.y + q1.w * q1.z);
this.m20 = 2.0f * (q1.x * q1.z - q1.w * q1.y);
this.m01 = 2.0f * (q1.x * q1.y - q1.w * q1.z);
this.m11 = 1.0f - 2.0f * q1.x * q1.x - 2.0f * q1.z * q1.z;
this.m21 = 2.0f * (q1.y * q1.z + q1.w * q1.x);
this.m02 = 2.0f * (q1.x * q1.z + q1.w * q1.y);
this.m12 = 2.0f * (q1.y * q1.z - q1.w * q1.x);
this.m22 = 1.0f - 2.0f * q1.x * q1.x - 2.0f * q1.y * q1.y;
}
/// <summary>
/// Sets the value of this axis-angle to the rotational equivalent
/// of the passed quaternion.
/// </summary>
/// <remarks>
/// Sets the value of this axis-angle to the rotational equivalent
/// of the passed quaternion.
/// If the specified quaternion has no rotational component, the value
/// of this AxisAngle4d is set to an angle of 0 about an axis of (0,1,0).
/// </remarks>
/// <param name="q1">the Quat4f</param>
public void Set(Quat4f q1)
{
double mag = q1.x * q1.x + q1.y * q1.y + q1.z * q1.z;
if (mag > Eps)
{
mag = Math.Sqrt(mag);
double invMag = 1.0 / mag;
x = q1.x * invMag;
y = q1.y * invMag;
z = q1.z * invMag;
angle = 2.0 * Math.Atan2(mag, q1.w);
}
else
{
x = 0.0f;
y = 1.0f;
z = 0.0f;
angle = 0.0f;
}
}
/// <summary>
/// Rotates the Camera so that its upVector() becomes \p up (defined in the world coordinate system).
/// The Camera is rotated around an axis orthogonal to \p up and to the current upVector() direction.
/// Use this method in order to define the Camera horizontal plane.
///
/// When \p noMove is set to \c false, the orientation modification is compensated by a translation, so
/// that the revolveAroundPoint() stays projected at the same position on screen. This is especially
/// useful when the Camera is an observer of the scene (default mouse binding).
///
/// When \p noMove is \c true (default), the Camera position() is left unchanged, which is an intuitive
/// behavior when the Camera is in a walkthrough fly mode (see the QGLViewer::MOVE_FORWARD and
/// QGLViewer::MOVE_BACKWARD QGLViewer::MouseAction).
/// </summary>
/// <param name="up"></param>
/// <param name="noMove"></param>
public void SetUpVector( Vector3f up, bool noMove )
{
var q = new Quat4f( new Vector3f( 0, 1, 0 ), Frame.TransformOf( up ) );
if( !noMove )
{
Frame.Position = RevolveAroundPoint - ( Frame.Orientation * q ).Rotate( Frame.CoordinatesOf( RevolveAroundPoint ) );
}
Frame.Rotate( q );
// Useful in fly mode to keep the horizontal direction.
Frame.UpdateFlyUpVector();
}
/// <summary>
/// Constructs and initializes a Matrix4d from the quaternion,
/// translation, and scale values; the scale is applied only to the
/// rotational components of the matrix (upper 3x3) and not to the
/// translational components.
/// </summary>
/// <remarks>
/// Constructs and initializes a Matrix4d from the quaternion,
/// translation, and scale values; the scale is applied only to the
/// rotational components of the matrix (upper 3x3) and not to the
/// translational components.
/// </remarks>
/// <param name="q1">the quaternion value representing the rotational component</param>
/// <param name="t1">the translational component of the matrix</param>
/// <param name="s">the scale value applied to the rotational components</param>
public Matrix4d(Quat4f q1, Vector3d t1, double s)
{
m00 = s * (1.0 - 2.0 * q1.y * q1.y - 2.0 * q1.z * q1.z);
m10 = s * (2.0 * (q1.x * q1.y + q1.w * q1.z));
m20 = s * (2.0 * (q1.x * q1.z - q1.w * q1.y));
m01 = s * (2.0 * (q1.x * q1.y - q1.w * q1.z));
m11 = s * (1.0 - 2.0 * q1.x * q1.x - 2.0 * q1.z * q1.z);
m21 = s * (2.0 * (q1.y * q1.z + q1.w * q1.x));
m02 = s * (2.0 * (q1.x * q1.z + q1.w * q1.y));
m12 = s * (2.0 * (q1.y * q1.z - q1.w * q1.x));
m22 = s * (1.0 - 2.0 * q1.x * q1.x - 2.0 * q1.y * q1.y);
m03 = t1.x;
m13 = t1.y;
m23 = t1.z;
m30 = 0.0;
m31 = 0.0;
m32 = 0.0;
m33 = 1.0;
}
