|
public static MulTransUnsafe ( |
||
| q | ||
| r | ||
| result | ||
| return | void | |
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;
}
|
Rot::MulTransUnsafe ( |
public static void MulTransToOutUnsafe(Transform A, Transform B, Transform result)
{
Debug.Assert(result != A);
Debug.Assert(result != B);
Rot.MulTransUnsafe(A.Q, B.Q, result.Q);
pool.Set(B.P).SubLocal(A.P);
Rot.MulTransUnsafe(A.Q, pool, result.P);
}
