public static void mulToOutUnsafe(Rot q, Vec2 v, Vec2 out_Renamed)
{
out_Renamed.x = q.c * v.x - q.s * v.y;
out_Renamed.y = q.s * v.x + q.c * v.y;
}
/// <summary>Initialize as a copy of another transform. </summary>
public Transform(Transform xf)
{
p = xf.p.Clone();
q = xf.q.Clone();
}
/// <summary>Initialize using a position vector and a rotation matrix. </summary>
public Transform(Vec2 _position, Rot _R)
{
p = _position.Clone();
q = _R.Clone();
}
public static void mul(Rot q, Rot r, Rot out_Renamed)
{
float tempc = q.c * r.c - q.s * r.s;
out_Renamed.s = q.s * r.c + q.c * r.s;
out_Renamed.c = tempc;
}
public static void MulTransUnsafe(Rot q, Rot r, Rot result)
{
// [ qc qs] * [rc -rs] = [qc*rc+qs*rs -qc*rs+qs*rc]
// [-qs qc] [rs rc] [-qs*rc+qc*rs qs*rs+qc*rc]
// s = qc * rs - qs * rc
// c = qc * rc + qs * rs
result.Sin = q.Cos * r.Sin - q.Sin * r.Cos;
result.Cos = q.Cos * r.Cos + q.Sin * r.Sin;
}
public static void mulTransUnsafe(Rot q, Vec2 v, Vec2 out_Renamed)
{
out_Renamed.x = q.c * v.x + q.s * v.y;
out_Renamed.y = (-q.s) * v.x + q.c * v.y;
}
public virtual Rot Clone()
{
Rot copy = new Rot();
copy.s = s;
copy.c = c;
return copy;
}
public static void MulToOutUnsafe(Rot q, Vec2 v, Vec2 result)
{
result.X = q.Cos * v.X - q.Sin * v.Y;
result.Y = q.Sin * v.X + q.Cos * v.Y;
}
public static void MulTrans(Rot q, Rot r, Rot result)
{
float tempc = q.Cos * r.Cos + q.Sin * r.Sin;
result.Sin = q.Cos * r.Sin - q.Sin * r.Cos;
result.Cos = tempc;
}
public Rot Set(Rot other)
{
Sin = other.Sin;
Cos = other.Cos;
return this;
}
public static void MulToOut(Rot q, Vec2 v, Vec2 result)
{
float tempy = q.Sin * v.X + q.Cos * v.Y;
result.X = q.Cos * v.X - q.Sin * v.Y;
result.Y = tempy;
}
public Rot Clone()
{
Rot copy = new Rot();
copy.Sin = Sin;
copy.Cos = Cos;
return copy;
}
public static void MulUnsafe(Rot q, Rot r, Rot result)
{
Debug.Assert(r != result);
Debug.Assert(q != result);
// [qc -qs] * [rc -rs] = [qc*rc-qs*rs -qc*rs-qs*rc]
// [qs qc] [rs rc] [qs*rc+qc*rs -qs*rs+qc*rc]
// s = qs * rc + qc * rs
// c = qc * rc - qs * rs
result.Sin = q.Sin * r.Cos + q.Cos * r.Sin;
result.Cos = q.Cos * r.Cos - q.Sin * r.Sin;
}
public static void MulTransUnsafe(Rot q, Vec2 v, Vec2 result)
{
result.X = q.Cos * v.X + q.Sin * v.Y;
result.Y = (-q.Sin) * v.X + q.Cos * v.Y;
}
public static void mulTrans(Rot q, Vec2 v, Vec2 out_Renamed)
{
float tempy = (-q.s) * v.x + q.c * v.y;
out_Renamed.x = q.c * v.x + q.s * v.y;
out_Renamed.y = tempy;
}
/// <summary>
/// The default constructor.
/// </summary>
public Transform()
{
P = new Vec2();
Q = new Rot();
}
public static void mulTransUnsafe(Rot q, Rot r, Rot out_Renamed)
{
// [ qc qs] * [rc -rs] = [qc*rc+qs*rs -qc*rs+qs*rc]
// [-qs qc] [rs rc] [-qs*rc+qc*rs qs*rs+qc*rc]
// s = qc * rs - qs * rc
// c = qc * rc + qs * rs
out_Renamed.s = q.c * r.s - q.s * r.c;
out_Renamed.c = q.c * r.c + q.s * r.s;
}
/// <summary>
/// Initialize as a copy of another transform.
/// </summary>
public Transform(Transform xf)
{
P = xf.P.Clone();
Q = xf.Q.Clone();
}
public static void mulUnsafe(Rot q, Rot r, Rot out_Renamed)
{
Debug.Assert(r != out_Renamed);
Debug.Assert(q != out_Renamed);
// [qc -qs] * [rc -rs] = [qc*rc-qs*rs -qc*rs-qs*rc]
// [qs qc] [rs rc] [qs*rc+qc*rs -qs*rs+qc*rc]
// s = qs * rc + qc * rs
// c = qc * rc - qs * rs
out_Renamed.s = q.s * r.c + q.c * r.s;
out_Renamed.c = q.c * r.c - q.s * r.s;
}
/// <summary>
/// Initialize using a position vector and a rotation matrix.
/// </summary>
public Transform(Vec2 position, Rot r)
{
P = position.Clone();
Q = r.Clone();
}
public virtual Rot set_Renamed(Rot other)
{
s = other.s;
c = other.c;
return this;
}
/// <summary>The default constructor. </summary>
public Transform()
{
p = new Vec2();
q = new Rot();
}
public static void mulToOut(Rot q, Vec2 v, Vec2 out_Renamed)
{
float tempy = q.s * v.x + q.c * v.y;
out_Renamed.x = q.c * v.x - q.s * v.y;
out_Renamed.y = tempy;
}
public static void MulTrans(Rot q, Vec2 v, Vec2 result)
{
float tempy = (-q.Sin) * v.X + q.Cos * v.Y;
result.X = q.Cos * v.X + q.Sin * v.Y;
result.Y = tempy;
}
