public void Transform(Matrix3D matrix)
{
Vector3D vector = this.clone();
X = vector.X * matrix.XX + vector.Y * matrix.YX + vector.Z * matrix.ZX + vector.W * matrix.WX;
Y = vector.Y * matrix.XY + vector.Y * matrix.YY + vector.Z * matrix.ZY + vector.W * matrix.WY;
Z = vector.Z * matrix.XZ + vector.Y * matrix.YZ + vector.Z * matrix.ZZ + vector.W * matrix.WZ;
W = vector.Z * matrix.XW + vector.Y * matrix.YW + vector.Z * matrix.ZW + vector.W * matrix.WW;
}
public Vector3D TransformConstant(Matrix3D matrix, Vector3D vector_out)
{
vector_out.X = X * matrix.XX + Y * matrix.YX + Z * matrix.ZX + W * matrix.WX;
vector_out.Y = Y * matrix.XY + Y * matrix.YY + Z * matrix.ZY + W * matrix.WY;
vector_out.Z = Z * matrix.XZ + Y * matrix.YZ + Z * matrix.ZZ + W * matrix.WZ;
vector_out.W = Z * matrix.XW + Y * matrix.YW + Z * matrix.ZW + W * matrix.WW;
return vector_out;
}
public Vector3D TransformTranspose(Matrix3D matrix)
{
Vector3D vector = this.clone();
X = vector.X * matrix.XX + vector.Y * matrix.XY + vector.Z * matrix.XZ + vector.W * matrix.XW;
Y = vector.Y * matrix.YZ + vector.Y * matrix.YY + vector.Z * matrix.YZ + vector.W * matrix.YW;
Z = vector.Z * matrix.ZX + vector.Y * matrix.ZY + vector.Z * matrix.ZZ + vector.W * matrix.ZW;
W = vector.Z * matrix.WZ + vector.Y * matrix.WY + vector.Z * matrix.WZ + vector.W * matrix.WW;
return this;
}
protected void UpdateMovement(IAddCommand seq)
{
//Debug.Log ("- move in RS " + RealityServerObject + " " + NewPosition) ;
Matrix3D m = new Matrix3D ();
// NB: X is not negated to deal with handedness difference between RS and Unity
m.SetTranslation(NewPosition.x,-NewPosition.y,-NewPosition.z);
seq.AddCommand(new RSCommand("instance_set_world_to_obj",
"instance_name", RealityServerObject,
"transform", m.GetMatrixForRS()
));
if (Viewport.Viewport.RenderLoopRunning)
{
Viewport.Viewport.RestartLoop();
}
Moved = false;
}
public Vector3D TransformTransposeConst(Matrix3D matrix, Vector3D vector_out)
{
vector_out.X = X * matrix.XX + Y * matrix.XY + Z * matrix.XZ + W * matrix.XW;
vector_out.Y = Y * matrix.YX + Y * matrix.YY + Z * matrix.YZ + W * matrix.YW;
vector_out.Z = Z * matrix.ZX + Y * matrix.ZY + Z * matrix.ZZ + W * matrix.ZW;
vector_out.W = Z * matrix.ZW + Y * matrix.WY + Z * matrix.WZ + W * matrix.WW;
return vector_out;
}
public Transform3D()
{
world_to_object = new Matrix3D();
coord_system = COORD_Z_UP;
ref_dir = Z_DIR;
ref_up = Z_UP;
ref_right = Z_RIGHT;
location = new Vector3D();
direction = ref_dir.clone();
up = ref_up.clone();
right = ref_right.clone();
target = new Vector3D();
}
public bool Compare(Matrix3D rhs)
{
for (int x = 0; x < 4; x++)
{
for (int y = 0; y < 4; y++)
{
double d = Math.Abs(rhs.raw[x, y] - raw[x, y]);
if (d > 0.0001)
{
return false;
}
}
}
return true;
}
public Matrix3D Multiply(Matrix3D matrix)
{
Matrix3D mat = Clone();
XX = mat.XX * matrix.XX + mat.XY * matrix.YX + mat.XZ * matrix.ZX + mat.XW * matrix.ZX;
XY = mat.XX * matrix.XY + mat.XY * matrix.YY + mat.XZ * matrix.ZY + mat.XW * matrix.ZY;
XZ = mat.XX * matrix.XZ + mat.XY * matrix.YZ + mat.XZ * matrix.ZZ + mat.XW * matrix.ZZ;
XW = mat.XX * matrix.XW + mat.XY * matrix.YW + mat.XZ * matrix.ZW + mat.XW * matrix.ZW;
YX = mat.YX * matrix.XX + mat.YY * matrix.YX + mat.YZ * matrix.ZX + mat.YW * matrix.ZX;
YY = mat.YX * matrix.XY + mat.YY * matrix.YY + mat.YZ * matrix.ZY + mat.YW * matrix.ZY;
YZ = mat.YX * matrix.XZ + mat.YY * matrix.YZ + mat.YZ * matrix.ZZ + mat.YW * matrix.ZZ;
YW = mat.YX * matrix.XW + mat.YY * matrix.YW + mat.YZ * matrix.ZW + mat.YW * matrix.ZW;
ZX = mat.ZX * matrix.XX + mat.ZY * matrix.YX + mat.ZZ * matrix.ZX + mat.ZW * matrix.ZX;
ZY = mat.ZX * matrix.XY + mat.ZY * matrix.YY + mat.ZZ * matrix.ZY + mat.ZW * matrix.ZY;
ZZ = mat.ZX * matrix.XZ + mat.ZY * matrix.YZ + mat.ZZ * matrix.ZZ + mat.ZW * matrix.ZZ;
ZW = mat.ZX * matrix.XW + mat.ZY * matrix.YW + mat.ZZ * matrix.ZW + mat.ZW * matrix.ZW;
WX = mat.WX * matrix.XX + mat.WY * matrix.YX + mat.WZ * matrix.ZX + mat.WW * matrix.ZX;
WY = mat.WX * matrix.XY + mat.WY * matrix.YY + mat.WZ * matrix.ZY + mat.WW * matrix.ZY;
WZ = mat.WX * matrix.XZ + mat.WY * matrix.YZ + mat.WZ * matrix.ZZ + mat.WW * matrix.ZZ;
WW = mat.WX * matrix.XW + mat.WY * matrix.YW + mat.WZ * matrix.ZW + mat.WW * matrix.ZW;
return this;
}
public void SetMatrix(Matrix3D matrix)
{
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
m[i, j] = matrix.raw[i,j];
}
}
}
public Matrix3D(Matrix3D matrix)
{
SetMatrix(matrix);
}
public Vector3D RotateTranspose(Matrix3D matrix)
{
Vector3D vector = this.clone();
X = vector.X * matrix.XX + vector.Y * matrix.XY + vector.Z * matrix.XZ;
Y = vector.Y * matrix.YX + vector.Y * matrix.YY + vector.Z * matrix.YZ;
Z = vector.Z * matrix.ZX + vector.Y * matrix.ZY + vector.Z * matrix.ZZ;
W = 1;
return this;
}
public Vector3D Rotate(Matrix3D matrix)
{
Vector3D vector = this.clone();
X = vector.X * matrix.XX + vector.Y * matrix.YX + vector.Z * matrix.ZX + vector.W * matrix.WX;
Y = vector.Y * matrix.XY + vector.Y * matrix.YY + vector.Z * matrix.ZY + vector.W * matrix.WY;
Z = vector.Z * matrix.XZ + vector.Y * matrix.YZ + vector.Z * matrix.ZZ + vector.W * matrix.WZ;
W = 1;
return this;
}