public void ReportCapture_ReportsProperJson()
{
var filename = "my/file.png";
var egoPosition = new float3(.02f, .03f, .04f);
var egoRotation = new quaternion(.1f, .2f, .3f, .4f);
var egoVelocity = new Vector3(.1f, .2f, .3f);
var position = new float3(.2f, 1.1f, .3f);
var rotation = new quaternion(.3f, .2f, .1f, .5f);
var intrinsics = new float3x3(.1f, .2f, .3f, 1f, 2f, 3f, 10f, 20f, 30f);
var capturesJsonExpected =
$@"{{
""version"": ""{DatasetCapture.SchemaVersion}"",
""captures"": [
{{
""id"": <guid>,
""sequence_id"": <guid>,
""step"": 0,
""timestamp"": 0.0,
""sensor"": {{
""sensor_id"": <guid>,
""ego_id"": <guid>,
""modality"": ""camera"",
""translation"": [
{Format(position.x)},
{Format(position.y)},
{Format(position.z)}
],
""rotation"": [
{Format(rotation.value.x)},
{Format(rotation.value.y)},
{Format(rotation.value.z)},
{Format(rotation.value.w)}
],
""camera_intrinsic"": [
[
{Format(intrinsics.c0.x)},
{Format(intrinsics.c0.y)},
{Format(intrinsics.c0.z)}
],
[
{Format(intrinsics.c1.x)},
{Format(intrinsics.c1.y)},
{Format(intrinsics.c1.z)}
],
[
{Format(intrinsics.c2.x)},
{Format(intrinsics.c2.y)},
{Format(intrinsics.c2.z)}
]
]
}},
""ego"": {{
""ego_id"": <guid>,
""translation"": [
{Format(egoPosition.x)},
{Format(egoPosition.y)},
{Format(egoPosition.z)}
],
""rotation"": [
{Format(egoRotation.value.x)},
{Format(egoRotation.value.y)},
{Format(egoRotation.value.z)},
{Format(egoRotation.value.w)}
],
""velocity"": [
{Format(egoVelocity.x)},
{Format(egoVelocity.y)},
{Format(egoVelocity.z)}
],
""acceleration"": null
}},
""filename"": ""{filename}"",
""format"": ""PNG""
}}
]
}}";
var ego = DatasetCapture.RegisterEgo("");
var sensorHandle = DatasetCapture.RegisterSensor(ego, "camera", "", 1, 0);
var sensorSpatialData = new SensorSpatialData(new Pose(egoPosition, egoRotation), new Pose(position, rotation), egoVelocity, null);
sensorHandle.ReportCapture(filename, sensorSpatialData, ("camera_intrinsic", intrinsics));
DatasetCapture.ResetSimulation();
Assert.IsFalse(sensorHandle.IsValid);
var capturesPath = Path.Combine(DatasetCapture.OutputDirectory, "captures_000.json");
FileAssert.Exists(capturesPath);
AssertJsonFileEquals(capturesJsonExpected, capturesPath);
}
public void SetRotate(float3x3 rotate)
{
fIsIdentity = false;
fIsRSMatrix = true;
fMatrix = rotate;
}
public Transform2D()
{
fMatrix = new XFORM();
fTransformMatrix = new float3x3();
// Start out with identity matrix
SetToIdentity();
}
public Transformation()
{
fMatrix = float3x3.Identity;
fTranslate = float3.Zero;
fScale = new float3(1, 1, 1);
fIsIdentity = true;
fIsRSMatrix = true;
fIsUniformScale = true;
}
public Transformation(Transformation aTrans)
{
fMatrix = aTrans.fMatrix;
fTranslate = aTrans.fTranslate;
fScale = aTrans.fScale;
fIsIdentity = aTrans.fIsIdentity;
fIsRSMatrix = aTrans.fIsRSMatrix;
fIsUniformScale = aTrans.fIsUniformScale;
}
// Reset to identity transform
public void SetToIdentity()
{
fTransformMatrix = float3x3.Identity;
fMatrix.eM11 = 1F;
fMatrix.eM12 = 0F;
fMatrix.eM21 = 0F;
fMatrix.eM22 = 1F;
fMatrix.eDx = 0F;
fMatrix.eDy = 0F;
}
public void Update(RigidTransform worldFromA, RigidTransform worldFromB, bool lockBtoA, float3 pivotA, float3 pivotB,
ref float3 directionA, ref float3 directionB, ref float3 perpendicularA, ref float3 perpendicularB, Object target)
{
// Work in world space
float3 directionAinW = math.rotate(worldFromA, directionA);
float3 directionBinW = math.rotate(worldFromB, directionB);
float3 perpendicularAinW = math.rotate(worldFromB, perpendicularA);
float3 perpendicularBinW = math.rotate(worldFromB, perpendicularB);
bool changed = false;
// If the target changed, fix up the inputs and reset the reference orientations to align with the new target's axes
if (target != m_LastTarget)
{
m_LastTarget = target;
// Enforce normalized directions
changed |= NormalizeSafe(ref directionAinW);
changed |= NormalizeSafe(ref directionBinW);
// Enforce normalized perpendiculars, orthogonal to their respective directions
changed |= NormalizePerpendicular(directionAinW, ref perpendicularAinW);
changed |= NormalizePerpendicular(directionBinW, ref perpendicularBinW);
// Calculate the rotation of the joint in A from direction and perpendicular
float3x3 rotationA = new float3x3(directionAinW, perpendicularAinW, math.cross(directionAinW, perpendicularAinW));
m_RefA = new quaternion(rotationA);
if (lockBtoA)
{
m_RefB = m_RefA;
}
else
{
// Calculate the rotation of the joint in B from direction and perpendicular
float3x3 rotationB = new float3x3(directionBinW, perpendicularBinW, math.cross(directionBinW, perpendicularBinW));
m_RefB = new quaternion(rotationB);
}
}
EditorGUI.BeginChangeCheck();
// Make rotators
quaternion oldRefA = m_RefA;
quaternion oldRefB = m_RefB;
float3 pivotAinW = math.transform(worldFromA, pivotA);
m_RefA = Handles.RotationHandle(m_RefA, pivotAinW);
float3 pivotBinW;
if (lockBtoA)
{
directionB = math.rotate(math.inverse(worldFromB), directionAinW);
perpendicularB = math.rotate(math.inverse(worldFromB), perpendicularAinW);
pivotBinW = pivotAinW;
m_RefB = m_RefA;
}
else
{
pivotBinW = math.transform(worldFromB, pivotB);
m_RefB = Handles.RotationHandle(m_RefB, pivotBinW);
}
// Apply changes from the rotators
if (EditorGUI.EndChangeCheck())
{
quaternion dqA = math.mul(m_RefA, math.inverse(oldRefA));
quaternion dqB = math.mul(m_RefB, math.inverse(oldRefB));
directionAinW = math.mul(dqA, directionAinW);
directionBinW = math.mul(dqB, directionBinW);
perpendicularAinW = math.mul(dqB, perpendicularAinW);
perpendicularBinW = math.mul(dqB, perpendicularBinW);
changed = true;
}
// Write back if the axes changed
if (changed)
{
Undo.RecordObject(target, "Edit joint axis");
directionA = math.rotate(math.inverse(worldFromA), directionAinW);
directionB = math.rotate(math.inverse(worldFromB), directionBinW);
perpendicularA = math.rotate(math.inverse(worldFromB), perpendicularAinW);
perpendicularB = math.rotate(math.inverse(worldFromB), perpendicularBinW);
}
// Draw the updated axes
float3 z = new float3(0, 0, 1); // ArrowHandleCap() draws an arrow pointing in (0, 0, 1)
Handles.ArrowHandleCap(0, pivotAinW, Quaternion.FromToRotation(z, directionAinW), HandleUtility.GetHandleSize(pivotAinW) * 0.75f, Event.current.type);
Handles.ArrowHandleCap(0, pivotAinW, Quaternion.FromToRotation(z, perpendicularAinW), HandleUtility.GetHandleSize(pivotAinW) * 0.75f, Event.current.type);
if (!lockBtoA)
{
Handles.ArrowHandleCap(0, pivotBinW, Quaternion.FromToRotation(z, directionBinW), HandleUtility.GetHandleSize(pivotBinW) * 0.75f, Event.current.type);
Handles.ArrowHandleCap(0, pivotBinW, Quaternion.FromToRotation(z, perpendicularBinW), HandleUtility.GetHandleSize(pivotBinW) * 0.75f, Event.current.type);
}
}
public void MakeIdentity()
{
fMatrix = float3x3.Identity;
fTranslate = float3.Zero;
fScale = new float3(1.0f,1.0f,1.0f);
fIsIdentity = true;
fIsRSMatrix = true;
fIsUniformScale = true;
}
public static void AreEqual(float3x3 a, float3x3 b, float delta = 0.0f)
{
AreEqual(a.c0, b.c0, delta);
AreEqual(a.c1, b.c1, delta);
AreEqual(a.c2, b.c2, delta);
}
public void Parse_ToString_InvariantCulture()
{
float3x3 f = new float3x3(1.5f, 0, 0, 0, 1.5f, 0, 0, 0, 1);
Assert.Equal(f, float3x3.Parse(f.ToString(CultureInfo.InvariantCulture), CultureInfo.InvariantCulture));
}
public void Transform_Float2Matrix_Operator(float3x3 mat, float2 vec, float2 expected)
{
var actual = vec * mat;
Assert.Equal(expected, -actual);
}
public void Mult_Operator(float3x3 left, float3x3 right, float3x3 expected)
{
var actual = left * right;
Assert.Equal(expected, actual);
}
void Update()
{
float deltaTime = Time.deltaTime;
FractalPart rootPart = parts[0][0];
rootPart.spinAngle += rootPart.spinVelocity * deltaTime;
rootPart.worldRotation = mul(transform.rotation,
mul(rootPart.rotation, quaternion.RotateY(rootPart.spinAngle)));
rootPart.worldPosition = transform.position;
parts[0][0] = rootPart;
float objectScale = transform.lossyScale.x;
float3x3 r = float3x3(rootPart.worldRotation) * objectScale;
matrices[0][0] = float3x4(r.c0, r.c1, r.c2, rootPart.worldPosition);
float scale = objectScale;
JobHandle jobHandle = default;
for (int li = 1; li < parts.Length; li++)
{
scale *= 0.5f;
jobHandle = new UpdateFractalLevelJob
{
deltaTime = deltaTime,
scale = scale,
parents = parts[li - 1],
parts = parts[li],
matrices = matrices[li]
}.ScheduleParallel(parts[li].Length, 5, jobHandle);
}
jobHandle.Complete();
var bounds = new Bounds(Vector3.zero, 3f * objectScale * Vector3.one);
int leafIndex = matricesBuffers.Length - 1;
for (int i = 0; i < matricesBuffers.Length; i++)
{
ComputeBuffer buffer = matricesBuffers[i];
buffer.SetData(matrices[i]);
Color colorA, colorB;
Mesh instanceMesh;
if (i == leafIndex)
{
colorA = leafColorA;
colorB = leafColorB;
instanceMesh = leafMesh;
}
else
{
float gradientInterpolator = i / (matricesBuffers.Length - 2f);
colorA = gradientA.Evaluate(gradientInterpolator);
colorB = gradientB.Evaluate(gradientInterpolator);
instanceMesh = mesh;
}
propertyBlock.SetColor(colorAId, colorA);
propertyBlock.SetColor(colorBId, colorB);
propertyBlock.SetBuffer(matricesId, buffer);
propertyBlock.SetVector(sequenceNumbersId, sequenceNumbers[i]);
Graphics.DrawMeshInstancedProcedural(
instanceMesh, 0, material, bounds, buffer.count, propertyBlock);
}
}
private void Awake()
{
joint = GetComponent <BioJoint>();
_root = transform.root;
threshold = 0.1f;
/*
* Label the joints based one their names.
* isLeft = [-1,0,1]
* chainPos = [-1,0,1] , shoulder and thighs are assigned to 1, hands and feet are assigned to -1, rest is 0
*/
if (gameObject.name.Contains("left") ||
gameObject.name.Contains("Left") ||
gameObject.name.Contains("_l"))
{
isLeft = 1;
}
else if (gameObject.name.Contains("right") ||
gameObject.name.Contains("Right") ||
gameObject.name.Contains("_r"))
{
isLeft = -1;
}
else
{
isLeft = 0;
}
if (gameObject.name.Contains("shoulder") ||
gameObject.name.Contains("Shoulder") ||
gameObject.name.Contains("Thigh") ||
gameObject.name.Contains("thigh") ||
gameObject.name.Contains("UpLeg"))
{
chainPos = 1;
}
else if (gameObject.name.Contains("hand") ||
gameObject.name.Contains("Hand") ||
gameObject.name.Contains("Wrist") ||
gameObject.name.Contains("Ankle") ||
gameObject.name.Contains("Foot") ||
gameObject.name.Contains("foot"))
{
chainPos = -1;
}
else
{
chainPos = 0;
}
centerOfMass = _root.InverseTransformPoint(transform.position);
totalMass = 1000 * math.PI * math.pow(radius, 2) * length.magnitude;
inertiaObj = new float3x3(
new float3(ReturnInertia(0), 0, 0),
new float3(0, ReturnInertia(1), 0),
new float3(0, 0, ReturnInertia(2))
);
initPosition = _root.InverseTransformPoint(transform.position);
initRotation = _root.transform.rotation * transform.rotation;
initRotationEuler = _root.InverseTransformDirection(transform.rotation.eulerAngles);
initOrientation = new float3x3(
_root.InverseTransformDirection(transform.forward),
_root.InverseTransformDirection(transform.up),
_root.InverseTransformDirection(transform.right));
}
public static IRaycastGeometry <float3> Quadric(float3x3 Q, float3 P, float R)
{
return(new QuadricGeometry(Q, P, R));
}
public QuadricGeometry(float3x3 Q, float3 P, float R)
{
this.quadric = new Quadric(Q, P, R);
}
public static void quaternion_euler()
{
float3 test_angles = TestMatrix.test_angles;
quaternion q0 = quaternion.Euler(test_angles);
quaternion q0_xyz = quaternion.Euler(test_angles, RotationOrder.XYZ);
quaternion q0_xzy = quaternion.Euler(test_angles, RotationOrder.XZY);
quaternion q0_yxz = quaternion.Euler(test_angles, RotationOrder.YXZ);
quaternion q0_yzx = quaternion.Euler(test_angles, RotationOrder.YZX);
quaternion q0_zxy = quaternion.Euler(test_angles, RotationOrder.ZXY);
quaternion q0_zyx = quaternion.Euler(test_angles, RotationOrder.ZYX);
quaternion q1 = quaternion.Euler(test_angles.x, test_angles.y, test_angles.z);
quaternion q1_xyz = quaternion.Euler(test_angles.x, test_angles.y, test_angles.z, RotationOrder.XYZ);
quaternion q1_xzy = quaternion.Euler(test_angles.x, test_angles.y, test_angles.z, RotationOrder.XZY);
quaternion q1_yxz = quaternion.Euler(test_angles.x, test_angles.y, test_angles.z, RotationOrder.YXZ);
quaternion q1_yzx = quaternion.Euler(test_angles.x, test_angles.y, test_angles.z, RotationOrder.YZX);
quaternion q1_zxy = quaternion.Euler(test_angles.x, test_angles.y, test_angles.z, RotationOrder.ZXY);
quaternion q1_zyx = quaternion.Euler(test_angles.x, test_angles.y, test_angles.z, RotationOrder.ZYX);
float epsilon = 0.0001f;
TestUtils.AreEqual(q0, quaternion(-0.3133549f, 0.3435619f, 0.3899215f, 0.7948176f), epsilon);
TestUtils.AreEqual(q0_xyz, quaternion(-0.4597331f, 0.06979711f, 0.3899215f, 0.7948176f), epsilon);
TestUtils.AreEqual(q0_xzy, quaternion(-0.3133549f, 0.06979711f, 0.3899215f, 0.8630749f), epsilon);
TestUtils.AreEqual(q0_yxz, quaternion(-0.4597331f, 0.06979711f, 0.1971690f, 0.8630748f), epsilon);
TestUtils.AreEqual(q0_yzx, quaternion(-0.4597331f, 0.34356190f, 0.1971690f, 0.7948176f), epsilon);
TestUtils.AreEqual(q0_zxy, quaternion(-0.3133549f, 0.34356190f, 0.3899215f, 0.7948176f), epsilon);
TestUtils.AreEqual(q0_zyx, quaternion(-0.3133549f, 0.34356190f, 0.1971690f, 0.8630749f), epsilon);
TestUtils.AreEqual(q1, quaternion(-0.3133549f, 0.3435619f, 0.3899215f, 0.7948176f), epsilon);
TestUtils.AreEqual(q1_xyz, quaternion(-0.4597331f, 0.06979711f, 0.3899215f, 0.7948176f), epsilon);
TestUtils.AreEqual(q1_xzy, quaternion(-0.3133549f, 0.06979711f, 0.3899215f, 0.8630749f), epsilon);
TestUtils.AreEqual(q1_yxz, quaternion(-0.4597331f, 0.06979711f, 0.1971690f, 0.8630748f), epsilon);
TestUtils.AreEqual(q1_yzx, quaternion(-0.4597331f, 0.34356190f, 0.1971690f, 0.7948176f), epsilon);
TestUtils.AreEqual(q1_zxy, quaternion(-0.3133549f, 0.34356190f, 0.3899215f, 0.7948176f), epsilon);
TestUtils.AreEqual(q1_zyx, quaternion(-0.3133549f, 0.34356190f, 0.1971690f, 0.8630749f), epsilon);
float3x3 m0 = float3x3(q0);
float3x3 m0_xyz = float3x3(q0_xyz);
float3x3 m0_xzy = float3x3(q0_xzy);
float3x3 m0_yxz = float3x3(q0_yxz);
float3x3 m0_yzx = float3x3(q0_yzx);
float3x3 m0_zxy = float3x3(q0_zxy);
float3x3 m0_zyx = float3x3(q0_zyx);
float3x3 m1 = float3x3(q1);
float3x3 m1_xyz = float3x3(q1_xyz);
float3x3 m1_xzy = float3x3(q1_xzy);
float3x3 m1_yxz = float3x3(q1_yxz);
float3x3 m1_yzx = float3x3(q1_yzx);
float3x3 m1_zxy = float3x3(q1_zxy);
float3x3 m1_zyx = float3x3(q1_zyx);
TestUtils.AreEqual(m0, TestMatrix.test3x3_zxy, epsilon);
TestUtils.AreEqual(m0_xyz, TestMatrix.test3x3_xyz, epsilon);
TestUtils.AreEqual(m0_yzx, TestMatrix.test3x3_yzx, epsilon);
TestUtils.AreEqual(m0_zxy, TestMatrix.test3x3_zxy, epsilon);
TestUtils.AreEqual(m0_xzy, TestMatrix.test3x3_xzy, epsilon);
TestUtils.AreEqual(m0_yxz, TestMatrix.test3x3_yxz, epsilon);
TestUtils.AreEqual(m0_zyx, TestMatrix.test3x3_zyx, 0.0001f);
TestUtils.AreEqual(m1, TestMatrix.test3x3_zxy, epsilon);
TestUtils.AreEqual(m1_xyz, TestMatrix.test3x3_xyz, epsilon);
TestUtils.AreEqual(m1_yzx, TestMatrix.test3x3_yzx, epsilon);
TestUtils.AreEqual(m1_zxy, TestMatrix.test3x3_zxy, epsilon);
TestUtils.AreEqual(m1_xzy, TestMatrix.test3x3_xzy, epsilon);
TestUtils.AreEqual(m1_yxz, TestMatrix.test3x3_yxz, epsilon);
TestUtils.AreEqual(m1_zyx, TestMatrix.test3x3_zyx, epsilon);
}
public void float3x3_euler()
{
float3x3 m0_xyz = float3x3.eulerXYZ(test_angles);
float3x3 m0_xzy = float3x3.eulerXZY(test_angles);
float3x3 m0_yxz = float3x3.eulerYXZ(test_angles);
float3x3 m0_yzx = float3x3.eulerYZX(test_angles);
float3x3 m0_zxy = float3x3.eulerZXY(test_angles);
float3x3 m0_zyx = float3x3.eulerZYX(test_angles);
float3x3 m1 = float3x3.euler(test_angles);
float3x3 m1_xyz = float3x3.euler(test_angles, RotationOrder.XYZ);
float3x3 m1_xzy = float3x3.euler(test_angles, RotationOrder.XZY);
float3x3 m1_yxz = float3x3.euler(test_angles, RotationOrder.YXZ);
float3x3 m1_yzx = float3x3.euler(test_angles, RotationOrder.YZX);
float3x3 m1_zxy = float3x3.euler(test_angles, RotationOrder.ZXY);
float3x3 m1_zyx = float3x3.euler(test_angles, RotationOrder.ZYX);
float3x3 m2_xyz = float3x3.eulerXYZ(test_angles.x, test_angles.y, test_angles.z);
float3x3 m2_xzy = float3x3.eulerXZY(test_angles.x, test_angles.y, test_angles.z);
float3x3 m2_yxz = float3x3.eulerYXZ(test_angles.x, test_angles.y, test_angles.z);
float3x3 m2_yzx = float3x3.eulerYZX(test_angles.x, test_angles.y, test_angles.z);
float3x3 m2_zxy = float3x3.eulerZXY(test_angles.x, test_angles.y, test_angles.z);
float3x3 m2_zyx = float3x3.eulerZYX(test_angles.x, test_angles.y, test_angles.z);
float3x3 m3 = float3x3.euler(test_angles.x, test_angles.y, test_angles.z);
float3x3 m3_xyz = float3x3.euler(test_angles.x, test_angles.y, test_angles.z, RotationOrder.XYZ);
float3x3 m3_xzy = float3x3.euler(test_angles.x, test_angles.y, test_angles.z, RotationOrder.XZY);
float3x3 m3_yxz = float3x3.euler(test_angles.x, test_angles.y, test_angles.z, RotationOrder.YXZ);
float3x3 m3_yzx = float3x3.euler(test_angles.x, test_angles.y, test_angles.z, RotationOrder.YZX);
float3x3 m3_zxy = float3x3.euler(test_angles.x, test_angles.y, test_angles.z, RotationOrder.ZXY);
float3x3 m3_zyx = float3x3.euler(test_angles.x, test_angles.y, test_angles.z, RotationOrder.ZYX);
TestUtils.AreEqual(m0_xyz, test3x3_xyz, 0.0001f);
TestUtils.AreEqual(m0_yzx, test3x3_yzx, 0.0001f);
TestUtils.AreEqual(m0_zxy, test3x3_zxy, 0.0001f);
TestUtils.AreEqual(m0_xzy, test3x3_xzy, 0.0001f);
TestUtils.AreEqual(m0_yxz, test3x3_yxz, 0.0001f);
TestUtils.AreEqual(m0_zyx, test3x3_zyx, 0.0001f);
TestUtils.AreEqual(m1, test3x3_zxy, 0.0001f);
TestUtils.AreEqual(m1_xyz, test3x3_xyz, 0.0001f);
TestUtils.AreEqual(m1_yzx, test3x3_yzx, 0.0001f);
TestUtils.AreEqual(m1_zxy, test3x3_zxy, 0.0001f);
TestUtils.AreEqual(m1_xzy, test3x3_xzy, 0.0001f);
TestUtils.AreEqual(m1_yxz, test3x3_yxz, 0.0001f);
TestUtils.AreEqual(m1_zyx, test3x3_zyx, 0.0001f);
TestUtils.AreEqual(m2_xyz, test3x3_xyz, 0.0001f);
TestUtils.AreEqual(m2_yzx, test3x3_yzx, 0.0001f);
TestUtils.AreEqual(m2_zxy, test3x3_zxy, 0.0001f);
TestUtils.AreEqual(m2_xzy, test3x3_xzy, 0.0001f);
TestUtils.AreEqual(m2_yxz, test3x3_yxz, 0.0001f);
TestUtils.AreEqual(m2_zyx, test3x3_zyx, 0.0001f);
TestUtils.AreEqual(m3, test3x3_zxy, 0.0001f);
TestUtils.AreEqual(m3_xyz, test3x3_xyz, 0.0001f);
TestUtils.AreEqual(m3_yzx, test3x3_yzx, 0.0001f);
TestUtils.AreEqual(m3_zxy, test3x3_zxy, 0.0001f);
TestUtils.AreEqual(m3_xzy, test3x3_xzy, 0.0001f);
TestUtils.AreEqual(m3_yxz, test3x3_yxz, 0.0001f);
TestUtils.AreEqual(m3_zyx, test3x3_zyx, 0.0001f);
}
// transform = 1 / transform
// M = A * x + B
// Inv(M) = Inv(A) * x - Inv(A) * B
public bool Invert()
{
fTransformMatrix = fTransformMatrix.Inverse;
float det = fMatrix.eM11 * fMatrix.eM22 - fMatrix.eM21 * fMatrix.eM12;
if (det == 0)
return false;
XFORM old = new XFORM(fMatrix);
fMatrix.eM11 = old.eM22 / det;
fMatrix.eM12 = -old.eM12 / det;
fMatrix.eM21 = -old.eM21 / det;
fMatrix.eM22 = old.eM11 / det;
fMatrix.eDx = -(fMatrix.eM11 * old.eDx + fMatrix.eM21 * old.eDy);
fMatrix.eDy = -(fMatrix.eM12 * old.eDx + fMatrix.eM22 * old.eDy);
return true;
}
public void Transform_MatrixFloat2_Static(float3x3 mat, float2 vec, float2 expected)
{
var actual = float3x3.Transform(mat, vec);
Assert.Equal(expected, actual);
}
public void Transform_Float2Matrix_Static(float3x3 mat, float2 vec, float2 expected)
{
var actual = float3x3.Transform(vec, mat);
Assert.Equal(expected, -actual);
}
protected override void OnUpdate()
{
ComponentDataFromEntity <Translation> Positions = GetComponentDataFromEntity <Translation>(true);
ComponentDataFromEntity <Rotation> Rotations = GetComponentDataFromEntity <Rotation>(true);
ComponentDataFromEntity <PhysicsVelocity> Velocities = GetComponentDataFromEntity <PhysicsVelocity>();
ComponentDataFromEntity <PhysicsMass> Masses = GetComponentDataFromEntity <PhysicsMass>(true);
ComponentDataFromEntity <PhysicsMassOverride> MassOverrides = GetComponentDataFromEntity <PhysicsMassOverride>(true);
// If there's a pick job, wait for it to finish
if (m_PickSystem.PickJobHandle != null)
{
JobHandle.CombineDependencies(Dependency, m_PickSystem.PickJobHandle.Value).Complete();
}
// If there's a picked entity, drag it
MousePickSystem.SpringData springData = m_PickSystem.SpringDataRef.Value;
if (springData.Dragging)
{
Entity entity = springData.Entity;
if (!Masses.HasComponent(entity))
{
return;
}
PhysicsMass massComponent = Masses[entity];
PhysicsVelocity velocityComponent = Velocities[entity];
// if body is kinematic
// TODO: you should be able to rotate a body with infinite mass but finite inertia
if (massComponent.HasInfiniteMass || MassOverrides.HasComponent(entity) && MassOverrides[entity].IsKinematic != 0)
{
return;
}
var worldFromBody = new MTransform(Rotations[entity].Value, Positions[entity].Value);
// Body to motion transform
var bodyFromMotion = new MTransform(Masses[entity].InertiaOrientation, Masses[entity].CenterOfMass);
MTransform worldFromMotion = Mul(worldFromBody, bodyFromMotion);
// TODO: shouldn't damp where inertia mass or inertia
// Damp the current velocity
const float gain = 0.95f;
velocityComponent.Linear *= gain;
velocityComponent.Angular *= gain;
// Get the body and mouse points in world space
float3 pointBodyWs = Mul(worldFromBody, springData.PointOnBody);
float3 pointSpringWs = Camera.main.ScreenToWorldPoint(new Vector3(Input.mousePosition.x, Input.mousePosition.y, springData.MouseDepth));
// Calculate the required change in velocity
float3 pointBodyLs = Mul(Inverse(bodyFromMotion), springData.PointOnBody);
float3 deltaVelocity;
{
float3 pointDiff = pointBodyWs - pointSpringWs;
float3 relativeVelocityInWorld = velocityComponent.Linear + math.mul(worldFromMotion.Rotation, math.cross(velocityComponent.Angular, pointBodyLs));
const float elasticity = 0.1f;
const float damping = 0.5f;
deltaVelocity = -pointDiff * (elasticity / Time.DeltaTime) - damping * relativeVelocityInWorld;
}
// Build effective mass matrix in world space
// TODO how are bodies with inf inertia and finite mass represented
// TODO the aggressive damping is hiding something wrong in this code if dragging non-uniform shapes
float3x3 effectiveMassMatrix;
{
float3 arm = pointBodyWs - worldFromMotion.Translation;
var skew = new float3x3(
new float3(0.0f, arm.z, -arm.y),
new float3(-arm.z, 0.0f, arm.x),
new float3(arm.y, -arm.x, 0.0f)
);
// world space inertia = worldFromMotion * inertiaInMotionSpace * motionFromWorld
var invInertiaWs = new float3x3(
massComponent.InverseInertia.x * worldFromMotion.Rotation.c0,
massComponent.InverseInertia.y * worldFromMotion.Rotation.c1,
massComponent.InverseInertia.z * worldFromMotion.Rotation.c2
);
invInertiaWs = math.mul(invInertiaWs, math.transpose(worldFromMotion.Rotation));
float3x3 invEffMassMatrix = math.mul(math.mul(skew, invInertiaWs), skew);
invEffMassMatrix.c0 = new float3(massComponent.InverseMass, 0.0f, 0.0f) - invEffMassMatrix.c0;
invEffMassMatrix.c1 = new float3(0.0f, massComponent.InverseMass, 0.0f) - invEffMassMatrix.c1;
invEffMassMatrix.c2 = new float3(0.0f, 0.0f, massComponent.InverseMass) - invEffMassMatrix.c2;
effectiveMassMatrix = math.inverse(invEffMassMatrix);
}
// Calculate impulse to cause the desired change in velocity
float3 impulse = math.mul(effectiveMassMatrix, deltaVelocity);
// Clip the impulse
const float maxAcceleration = 250.0f;
float maxImpulse = math.rcp(massComponent.InverseMass) * Time.DeltaTime * maxAcceleration;
impulse *= math.min(1.0f, math.sqrt((maxImpulse * maxImpulse) / math.lengthsq(impulse)));
// Apply the impulse
{
velocityComponent.Linear += impulse * massComponent.InverseMass;
float3 impulseLs = math.mul(math.transpose(worldFromMotion.Rotation), impulse);
float3 angularImpulseLs = math.cross(pointBodyLs, impulseLs);
velocityComponent.Angular += angularImpulseLs * massComponent.InverseInertia;
}
// Write back velocity
Velocities[entity] = velocityComponent;
}
}
public void Transform_MatrixFloat2_Operator(float3x3 mat, float2 vec, float2 expected)
{
var actual = mat * vec;
Assert.Equal(expected, actual);
}
public static string Convert3x3ToString(float3x3 M)
{
return(string.Format("{0}f, {1}f, {2}f\n{3}f, {4}f, {5}f\n{6}f, {7}f, {8}f", M.c0.x, M.c0.y, M.c0.z, M.c1.x, M.c1.y, M.c1.z, M.c2.x, M.c2.y, M.c2.z));
}
public void Parse_ToString_NoCulture()
{
float3x3 f = new float3x3(1.5f, 0, 0, 0, 1.5f, 0, 0, 0, 1);
Assert.Equal(f, float3x3.Parse(f.ToString()));
}
public static float3x3 PolarDecomposition(float3x3 M, float tolerance)
{
const float epsilon = 1.0e-15f;
float3x3 mTranspose = math.transpose(M);
float mOne = OneNorm(M);
float mInf = InfNorm(M);
float3x3 MadjTt = new float3x3();
float3x3 Et = new float3x3();
float Eone;
do
{
MadjTt.c0 = math.cross(mTranspose.c1, mTranspose.c2);
MadjTt.c1 = math.cross(mTranspose.c2, mTranspose.c0);
MadjTt.c2 = math.cross(mTranspose.c0, mTranspose.c1);
float det = math.dot(MadjTt.c0, mTranspose.c0);
if (math.abs(det) < epsilon)
{
int index = int.MaxValue;
if (math.dot(MadjTt.c0, MadjTt.c0) > epsilon)
{
index = 0;
}
else if (math.dot(MadjTt.c1, MadjTt.c1) > epsilon)
{
index = 1;
}
else if (math.dot(MadjTt.c2, MadjTt.c2) > epsilon)
{
index = 2;
}
if (index == int.MaxValue)
{
return(float3x3.identity);
}
else
{
SetRow(index, ref mTranspose, math.cross(
GetRow((index + 1) % 3, mTranspose),
GetRow((index + 2) % 3, mTranspose)
));
SetRow((index + 1) % 3, ref MadjTt, math.cross(
GetRow((index + 2) % 3, mTranspose),
GetRow(index, mTranspose)
));
SetRow((index + 2) % 3, ref MadjTt, math.cross(
GetRow(index, mTranspose),
GetRow((index + 1) % 3, mTranspose)
));
float3x3 m2 = math.transpose(mTranspose);
mOne = OneNorm(m2);
mInf = InfNorm(m2);
det = math.dot(mTranspose.c0, MadjTt.c0);
}
}
float MadjTone = OneNorm(MadjTt);
float MadjTinf = InfNorm(MadjTt);
float gamma = math.sqrt(math.sqrt((MadjTone * MadjTinf) / (mOne * mInf)) / math.abs(det));
float g1 = gamma * 0.5f;
float g2 = 0.5f / (gamma * det);
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
SetValue(i, j, ref Et, GetValue(i, j, mTranspose));
SetValue(i, j, ref mTranspose, g1 * GetValue(i, j, mTranspose) + g2 * GetValue(i, j, MadjTt));
SetValue(i, j, ref Et, GetValue(i, j, Et) - GetValue(i, j, mTranspose));
}
}
Eone = OneNorm(Et);
mOne = OneNorm(mTranspose);
mInf = InfNorm(mTranspose);
} while(Eone > mOne * tolerance);
return(math.transpose(mTranspose));
}
public void Parse_ToString_CultureDE()
{
float3x3 f = new float3x3(1.5f, 0, 0, 0, 1.5f, 0, 0, 0, 1);
Assert.Equal(f, float3x3.Parse(f.ToString(new CultureInfo("de-DE")), new CultureInfo("de-DE")));
}
// Calculate the eigenvectors and eigenvalues of a symmetric 3x3 matrix
public static void DiagonalizeSymmetricApproximation(float3x3 a, out float3x3 eigenVectors, out float3 eigenValues)
{
float GetMatrixElement(float3x3 m, int row, int col)
{
switch (col)
{
case 0: return(m.c0[row]);
case 1: return(m.c1[row]);
case 2: return(m.c2[row]);
default: UnityEngine.Assertions.Assert.IsTrue(false); return(0.0f);
}
}
void SetMatrixElement(ref float3x3 m, int row, int col, float x)
{
switch (col)
{
case 0: m.c0[row] = x; break;
case 1: m.c1[row] = x; break;
case 2: m.c2[row] = x; break;
default: UnityEngine.Assertions.Assert.IsTrue(false); break;
}
}
eigenVectors = float3x3.identity;
float epsSq = 1e-14f * (math.lengthsq(a.c0) + math.lengthsq(a.c1) + math.lengthsq(a.c2));
const int maxIterations = 10;
for (int iteration = 0; iteration < maxIterations; iteration++)
{
// Find the row (p) and column (q) of the off-diagonal entry with greater magnitude
int p = 0, q = 1;
{
float maxEntry = math.abs(a.c1[0]);
float mag02 = math.abs(a.c2[0]);
float mag12 = math.abs(a.c2[1]);
if (mag02 > maxEntry)
{
maxEntry = mag02;
p = 0;
q = 2;
}
if (mag12 > maxEntry)
{
maxEntry = mag12;
p = 1;
q = 2;
}
// Terminate if it's small enough
if (maxEntry * maxEntry < epsSq)
{
break;
}
}
// Calculate jacobia rotation
float3x3 j = float3x3.identity;
{
float apq = GetMatrixElement(a, p, q);
float tau = (GetMatrixElement(a, q, q) - GetMatrixElement(a, p, p)) / (2.0f * apq);
float t = math.sqrt(1.0f + tau * tau);
if (tau > 0.0f)
{
t = 1.0f / (tau + t);
}
else
{
t = 1.0f / (tau - t);
}
float c = math.rsqrt(1.0f + t * t);
float s = t * c;
SetMatrixElement(ref j, p, p, c);
SetMatrixElement(ref j, q, q, c);
SetMatrixElement(ref j, p, q, s);
SetMatrixElement(ref j, q, p, -s);
}
// Rotate a
a = math.mul(math.transpose(j), math.mul(a, j));
eigenVectors = math.mul(eigenVectors, j);
}
eigenValues = new float3(a.c0.x, a.c1.y, a.c2.z);
}
public static void AreEqual(float3x3 a, float3x3 b, int maxUlp, bool signedZeroEqual)
{
AreEqual(a.c0, b.c0, maxUlp, signedZeroEqual);
AreEqual(a.c1, b.c1, maxUlp, signedZeroEqual);
AreEqual(a.c2, b.c2, maxUlp, signedZeroEqual);
}
protected void ControlCamera(Entity ecam)
{
// camera controls
var input = World.GetExistingSystem <InputSystem>();
float dt = (float)Time.DeltaTime;
var env = World.TinyEnvironment();
var di = env.GetConfigData <DisplayInfo>();
float2 inpos = input.GetInputPosition();
float2 deltaMouse = (inpos - m_prevMouse) / new float2(di.width, di.height);
m_prevMouse = inpos;
bool mb0 = input.GetMouseButton(0);
bool mb1 = input.GetMouseButton(1);
bool mb2 = input.GetMouseButton(2);
if (input.IsTouchSupported())
{
if (input.TouchCount() > 0)
{
Touch t0 = input.GetTouch(0);
deltaMouse.x = t0.deltaX / (float)di.width;
deltaMouse.y = t0.deltaY / (float)di.height;
}
if (input.TouchCount() == 1)
{
mb0 = true;
}
else if (input.TouchCount() == 2)
{
mb1 = true;
}
}
var tPos = EntityManager.GetComponentData <Translation>(ecam);
var tRot = EntityManager.GetComponentData <Rotation>(ecam);
var cam = EntityManager.GetComponentData <Camera>(ecam);
CameraKeyControl cc = default;
if (EntityManager.HasComponent <CameraKeyControl>(ecam))
{
cc = EntityManager.GetComponentData <CameraKeyControl>(ecam);
}
else
{
cc.Default();
}
float3x3 rMat = new float3x3(tRot.Value);
if (input.GetKey(KeyCode.UpArrow) || input.GetKey(KeyCode.W))
{
tPos.Value += rMat.c2 * cc.movespeed * dt;
}
if (input.GetKey(KeyCode.DownArrow) || input.GetKey(KeyCode.S))
{
tPos.Value -= rMat.c2 * cc.movespeed * dt;
}
if (input.GetKey(KeyCode.LeftArrow) || input.GetKey(KeyCode.A))
{
tPos.Value -= rMat.c0 * cc.movespeed * dt;
}
if (input.GetKey(KeyCode.RightArrow) || input.GetKey(KeyCode.D))
{
tPos.Value += rMat.c0 * cc.movespeed * dt;
}
if (input.GetKey(KeyCode.PageUp) || input.GetKey(KeyCode.R))
{
cam.fov += cc.fovspeed * dt;
}
if (input.GetKey(KeyCode.PageDown) || input.GetKey(KeyCode.F))
{
cam.fov -= cc.fovspeed * dt;
}
if (input.GetKey(KeyCode.Return))
{
tPos.Value = new float3(0, 0, -20.0f);
tRot.Value = quaternion.identity;
cam.fov = 60;
}
cam.fov = math.clamp(cam.fov, 0.1f, 179.0f);
if (mb0)
{
var dyAxis = quaternion.EulerXYZ(new float3(0, deltaMouse.x * math.radians(cc.mouserotspeed), 0));
var dxAxis = quaternion.EulerXYZ(new float3(-deltaMouse.y * math.radians(cc.mouserotspeed), 0, 0));
tRot.Value = math.mul(tRot.Value, dyAxis);
tRot.Value = math.mul(tRot.Value, dxAxis);
}
if (mb1)
{
tPos.Value += rMat.c0 * -deltaMouse.x * cc.mousemovespeed;
tPos.Value += rMat.c1 * deltaMouse.y * cc.mousemovespeed;
}
if (input.GetMouseButton(2))
{
tPos.Value += rMat.c2 * deltaMouse.y * cc.mousemovespeed;
}
// write back
EntityManager.SetComponentData <Translation>(ecam, tPos);
EntityManager.SetComponentData <Rotation>(ecam, tRot);
EntityManager.SetComponentData <Camera>(ecam, cam);
}
public void rigid_transform_euler()
{
float3 test_angles = TestMatrix.test_angles;
RigidTransform q0 = RigidTransform.euler(test_angles);
RigidTransform q0_xyz = RigidTransform.euler(test_angles, RotationOrder.XYZ);
RigidTransform q0_xzy = RigidTransform.euler(test_angles, RotationOrder.XZY);
RigidTransform q0_yxz = RigidTransform.euler(test_angles, RotationOrder.YXZ);
RigidTransform q0_yzx = RigidTransform.euler(test_angles, RotationOrder.YZX);
RigidTransform q0_zxy = RigidTransform.euler(test_angles, RotationOrder.ZXY);
RigidTransform q0_zyx = RigidTransform.euler(test_angles, RotationOrder.ZYX);
RigidTransform q1 = RigidTransform.euler(test_angles.x, test_angles.y, test_angles.z);
RigidTransform q1_xyz = RigidTransform.euler(test_angles.x, test_angles.y, test_angles.z, RotationOrder.XYZ);
RigidTransform q1_xzy = RigidTransform.euler(test_angles.x, test_angles.y, test_angles.z, RotationOrder.XZY);
RigidTransform q1_yxz = RigidTransform.euler(test_angles.x, test_angles.y, test_angles.z, RotationOrder.YXZ);
RigidTransform q1_yzx = RigidTransform.euler(test_angles.x, test_angles.y, test_angles.z, RotationOrder.YZX);
RigidTransform q1_zxy = RigidTransform.euler(test_angles.x, test_angles.y, test_angles.z, RotationOrder.ZXY);
RigidTransform q1_zyx = RigidTransform.euler(test_angles.x, test_angles.y, test_angles.z, RotationOrder.ZYX);
float epsilon = 0.0001f;
TestUtils.AreEqual(q0, RigidTransform(Quaternion(-0.3133549f, 0.3435619f, 0.3899215f, 0.7948176f), float3.zero), epsilon);
TestUtils.AreEqual(q0_xyz, RigidTransform(Quaternion(-0.4597331f, 0.06979711f, 0.3899215f, 0.7948176f), float3.zero), epsilon);
TestUtils.AreEqual(q0_xzy, RigidTransform(Quaternion(-0.3133549f, 0.06979711f, 0.3899215f, 0.8630749f), float3.zero), epsilon);
TestUtils.AreEqual(q0_yxz, RigidTransform(Quaternion(-0.4597331f, 0.06979711f, 0.1971690f, 0.8630748f), float3.zero), epsilon);
TestUtils.AreEqual(q0_yzx, RigidTransform(Quaternion(-0.4597331f, 0.34356190f, 0.1971690f, 0.7948176f), float3.zero), epsilon);
TestUtils.AreEqual(q0_zxy, RigidTransform(Quaternion(-0.3133549f, 0.34356190f, 0.3899215f, 0.7948176f), float3.zero), epsilon);
TestUtils.AreEqual(q0_zyx, RigidTransform(Quaternion(-0.3133549f, 0.34356190f, 0.1971690f, 0.8630749f), float3.zero), epsilon);
TestUtils.AreEqual(q1, RigidTransform(Quaternion(-0.3133549f, 0.3435619f, 0.3899215f, 0.7948176f), float3.zero), epsilon);
TestUtils.AreEqual(q1_xyz, RigidTransform(Quaternion(-0.4597331f, 0.06979711f, 0.3899215f, 0.7948176f), float3.zero), epsilon);
TestUtils.AreEqual(q1_xzy, RigidTransform(Quaternion(-0.3133549f, 0.06979711f, 0.3899215f, 0.8630749f), float3.zero), epsilon);
TestUtils.AreEqual(q1_yxz, RigidTransform(Quaternion(-0.4597331f, 0.06979711f, 0.1971690f, 0.8630748f), float3.zero), epsilon);
TestUtils.AreEqual(q1_yzx, RigidTransform(Quaternion(-0.4597331f, 0.34356190f, 0.1971690f, 0.7948176f), float3.zero), epsilon);
TestUtils.AreEqual(q1_zxy, RigidTransform(Quaternion(-0.3133549f, 0.34356190f, 0.3899215f, 0.7948176f), float3.zero), epsilon);
TestUtils.AreEqual(q1_zyx, RigidTransform(Quaternion(-0.3133549f, 0.34356190f, 0.1971690f, 0.8630749f), float3.zero), epsilon);
float3x3 m0 = float3x3(q0.rot);
float3x3 m0_xyz = float3x3(q0_xyz.rot);
float3x3 m0_xzy = float3x3(q0_xzy.rot);
float3x3 m0_yxz = float3x3(q0_yxz.rot);
float3x3 m0_yzx = float3x3(q0_yzx.rot);
float3x3 m0_zxy = float3x3(q0_zxy.rot);
float3x3 m0_zyx = float3x3(q0_zyx.rot);
float3x3 m1 = float3x3(q1.rot);
float3x3 m1_xyz = float3x3(q1_xyz.rot);
float3x3 m1_xzy = float3x3(q1_xzy.rot);
float3x3 m1_yxz = float3x3(q1_yxz.rot);
float3x3 m1_yzx = float3x3(q1_yzx.rot);
float3x3 m1_zxy = float3x3(q1_zxy.rot);
float3x3 m1_zyx = float3x3(q1_zyx.rot);
TestUtils.AreEqual(m0, TestMatrix.test3x3_zxy, epsilon);
TestUtils.AreEqual(m0_xyz, TestMatrix.test3x3_xyz, epsilon);
TestUtils.AreEqual(m0_yzx, TestMatrix.test3x3_yzx, epsilon);
TestUtils.AreEqual(m0_zxy, TestMatrix.test3x3_zxy, epsilon);
TestUtils.AreEqual(m0_xzy, TestMatrix.test3x3_xzy, epsilon);
TestUtils.AreEqual(m0_yxz, TestMatrix.test3x3_yxz, epsilon);
TestUtils.AreEqual(m0_zyx, TestMatrix.test3x3_zyx, 0.0001f);
TestUtils.AreEqual(m1, TestMatrix.test3x3_zxy, epsilon);
TestUtils.AreEqual(m1_xyz, TestMatrix.test3x3_xyz, epsilon);
TestUtils.AreEqual(m1_yzx, TestMatrix.test3x3_yzx, epsilon);
TestUtils.AreEqual(m1_zxy, TestMatrix.test3x3_zxy, epsilon);
TestUtils.AreEqual(m1_xzy, TestMatrix.test3x3_xzy, epsilon);
TestUtils.AreEqual(m1_yxz, TestMatrix.test3x3_yxz, epsilon);
TestUtils.AreEqual(m1_zyx, TestMatrix.test3x3_zyx, epsilon);
}
static bool IsNegative(this float3x3 m)
{
var cross = math.cross(m.c0, m.c1);
return(math.dot(cross, m.c2) < 0f);
}
public void Zero_IsZero()
{
var expected = new float3x3(0, 0, 0, 0, 0, 0, 0, 0, 0);
Assert.Equal(expected, float3x3.Zero);
}
public void Mult_Static(float3x3 left, float3x3 right, float3x3 expected)
{
var actual = float3x3.Mult(left, right);
Assert.Equal(expected, actual);
}
public unsafe void Execute()
{
// Color palette
Color colorA = Color.cyan;
Color colorB = Color.magenta;
Color colorError = Color.red;
Color colorRange = Color.yellow;
OutputStream.Begin(0);
for (int iJoint = 0; iJoint < Joints.Length; iJoint++)
{
Joint joint = Joints[iJoint];
JointData *jointData = (JointData *)joint.JointData.GetUnsafePtr();
if (!joint.BodyPair.IsValid)
{
continue;
}
RigidBody bodyA = Bodies[joint.BodyPair.BodyAIndex];
RigidBody bodyB = Bodies[joint.BodyPair.BodyBIndex];
MTransform worldFromA, worldFromB;
MTransform worldFromJointA, worldFromJointB;
{
worldFromA = new MTransform(bodyA.WorldFromBody);
worldFromB = new MTransform(bodyB.WorldFromBody);
worldFromJointA = Mul(worldFromA, jointData->AFromJoint);
worldFromJointB = Mul(worldFromB, jointData->BFromJoint);
}
float3 pivotA = worldFromJointA.Translation;
float3 pivotB = worldFromJointB.Translation;
foreach (Constraint constraint in jointData->Constraints)
{
switch (constraint.Type)
{
case ConstraintType.Linear:
float3 diff = pivotA - pivotB;
// Draw the feature on B and find the range for A
float3 rangeOrigin;
float3 rangeDirection;
float rangeDistance;
switch (constraint.Dimension)
{
case 1:
float3 normal = worldFromJointB.Rotation[constraint.ConstrainedAxis1D];
OutputStream.Plane(pivotB, normal * k_Scale, colorB);
rangeDistance = math.dot(normal, diff);
rangeOrigin = pivotA - normal * rangeDistance;
rangeDirection = normal;
break;
case 2:
float3 direction = worldFromJointB.Rotation[constraint.FreeAxis2D];
OutputStream.Line(pivotB - direction * k_Scale, pivotB + direction * k_Scale, colorB);
float dot = math.dot(direction, diff);
rangeOrigin = pivotB + direction * dot;
rangeDirection = diff - direction * dot;
rangeDistance = math.length(rangeDirection);
rangeDirection = math.select(rangeDirection / rangeDistance, float3.zero, rangeDistance < 1e-5);
break;
case 3:
OutputStream.Point(pivotB, k_Scale, colorB);
rangeOrigin = pivotB;
rangeDistance = math.length(diff);
rangeDirection = math.select(diff / rangeDistance, float3.zero, rangeDistance < 1e-5);
break;
default:
throw new NotImplementedException();
}
// Draw the pivot on A
OutputStream.Point(pivotA, k_Scale, colorA);
// Draw error
float3 rangeA = rangeOrigin + rangeDistance * rangeDirection;
float3 rangeMin = rangeOrigin + constraint.Min * rangeDirection;
float3 rangeMax = rangeOrigin + constraint.Max * rangeDirection;
if (rangeDistance < constraint.Min)
{
OutputStream.Line(rangeA, rangeMin, colorError);
}
else if (rangeDistance > constraint.Max)
{
OutputStream.Line(rangeA, rangeMax, colorError);
}
if (math.length(rangeA - pivotA) > 1e-5f)
{
OutputStream.Line(rangeA, pivotA, colorError);
}
// Draw the range
if (constraint.Min != constraint.Max)
{
OutputStream.Line(rangeMin, rangeMax, colorRange);
}
break;
case ConstraintType.Angular:
switch (constraint.Dimension)
{
case 1:
// Get the limited axis and perpendicular in joint space
int constrainedAxis = constraint.ConstrainedAxis1D;
float3 axisInWorld = worldFromJointA.Rotation[constrainedAxis];
float3 perpendicularInWorld = worldFromJointA.Rotation[(constrainedAxis + 1) % 3] * k_Scale;
// Draw the angle of A
OutputStream.Line(pivotA, pivotA + perpendicularInWorld, colorA);
// Calculate the relative angle
float angle;
{
float3x3 jointBFromA = math.mul(math.inverse(worldFromJointB.Rotation), worldFromJointA.Rotation);
angle = CalculateTwistAngle(new quaternion(jointBFromA), constrainedAxis);
}
// Draw the range in B
float3 axis = worldFromJointA.Rotation[constraint.ConstrainedAxis1D];
OutputStream.Arc(pivotB, axis, math.mul(quaternion.AxisAngle(axis, constraint.Min - angle), perpendicularInWorld), constraint.Max - constraint.Min, colorB);
break;
case 2:
// Get axes in world space
int axisIndex = constraint.FreeAxis2D;
float3 axisA = worldFromJointA.Rotation[axisIndex];
float3 axisB = worldFromJointB.Rotation[axisIndex];
// Draw the cones in B
if (constraint.Min == 0.0f)
{
OutputStream.Line(pivotB, pivotB + axisB * k_Scale, colorB);
}
else
{
OutputStream.Cone(pivotB, axisB * k_Scale, constraint.Min, colorB);
}
if (constraint.Max != constraint.Min)
{
OutputStream.Cone(pivotB, axisB * k_Scale, constraint.Max, colorB);
}
// Draw the axis in A
OutputStream.Arrow(pivotA, axisA * k_Scale, colorA);
break;
case 3:
// TODO - no idea how to visualize this if the limits are nonzero :)
break;
default:
throw new NotImplementedException();
}
break;
default:
throw new NotImplementedException();
}
}
}
OutputStream.End();
}
public void Identity_IsIdentity()
{
var expected = new float3x3(1, 0, 0, 0, 1, 0, 0, 0, 1);
Assert.Equal(expected, float3x3.Identity);
}
public static void GetLocalInertiaTensor(this Rigidbody rb, out float3x3 localInertiaTensor)
{
localInertiaTensor = math.mul(new float3x3(rb.inertiaTensorRotation), float3x3.Scale(rb.inertiaTensor));
}
public bool Rotate(float angle, float x0, float y0)
{
float3 unity = new float3(new float2(x0, y0));
unity.Unit();
fTransformMatrix = float3x3.Rotate(angle, unity);
XFORM xm = new XFORM();
Translate(-x0, -y0); // make (x0,y0) the origin
double rad = angle * (Math.PI / 180);
xm.eM11 = (float)Math.Cos(rad);
xm.eM12 = (float)Math.Sin(rad);
xm.eM21 = -xm.eM12;
xm.eM22 = xm.eM11;
xm.eDx = 0;
xm.eDy = 0;
Combine(xm); // rotate
Translate(x0, y0); // move origin back
return true;
}
