/// <summary>
/// Appends transform along to the given direction
/// (the object must have original direction along YAxis(!))
/// </summary>
/// <param name="originalMatrix">Matrix</param>
/// <param name="direction">Direction</param>
/// <returns>Transformation</returns>
public static Matrix3D TransformAlongTo(this Matrix3D originalMatrix, Vector3D direction)
{
Matrix3D matrix = new Matrix3D();
if ((!direction.ApproxEqual(new Vector3D(0,+1,0), 0.001)) &&
(!direction.ApproxEqual(new Vector3D(0,-1,0), 0.001)))
{
Vector3D firstVector = Vector3D.CrossProduct(new Vector3D(0,10,0), direction);
firstVector.Normalize();
// Получаем перпендикуляр к полученному ранее перепендикуляру
Vector3D secondVector = Vector3D.CrossProduct(firstVector, direction);
secondVector.Normalize();
// Получаем матрицу поворота
matrix.M11 = firstVector.X; matrix.M12 = firstVector.Y; matrix.M13 = firstVector.Z;
matrix.M21 = direction.X; matrix.M22 = direction.Y; matrix.M23 = direction.Z;
matrix.M31 = secondVector.X; matrix.M32 = secondVector.Y; matrix.M33 = secondVector.Z;
}
else if (direction.ApproxEqual(new Vector3D(0,-1,0), 0.001))
{
matrix.Scale(new Vector3D(1, -1, 1));
}
else return originalMatrix;
// Append transform
originalMatrix.Append(matrix);
return originalMatrix;
}
public void GetKMuForceTest1()
{
var me = new VelPosDummy(1, 2, 3, 0, 0, 0);
var toThis = new VelPosDummy(3, 4, 5, 0, 0, 0);
var diag = Math.Sqrt(3);
var f = Phys3DHelper.GetKMuForce(me, toThis, 10, 10, 2 * diag);
Assert.IsTrue(Vector3D.ApproxEqual(f, Vector3D.Zero, 0.00000001));
}
public void GetNForceTest4()
{
var surf = new RbSurfFloor(77, 44, new Vector3D(10, 20, 30));
var localPos = new Vector3D(-10, 19, 33);
var localVel = new Vector3D(0, -1000, 0);
var answ = surf.GetNForce(localPos, localVel);
var correctansw = new Vector3D(0, 77 + 1000 * 44, 0);
Assert.IsTrue(Vector3D.ApproxEqual(correctansw, answ));
}
public void GetNForceTest4()
{
var surf = new RbSurfAngleFloor(77, 44, new Vector3D(10, 20, 33), new Vector3D(10, 0, 0));
var localPos = new Vector3D(9, 19, 33);
var localVel = new Vector3D(-888, 0, 0);
var answ = surf.GetNForce(localPos, localVel);
var correctansw = new Vector3D(77 + 888 * 44, 0, 0);
Assert.IsTrue(Vector3D.ApproxEqual(correctansw, answ));
}
public void GetKMuForceTest3()
{
var me = new VelPosDummy(1, 2, 3, 0, 0, 0);
var toThis = new VelPosDummy(3, 4, 5, 0, 0, 0);
var diag = Math.Sqrt(3);
var f = Phys3DHelper.GetKMuForce(me, toThis, 10, 10, 2 * diag);
Assert.IsTrue(Vector3D.ApproxEqual(f, Vector3D.Zero, 0.00000001));
me.Vel.Vec3D = new Vector3D(1, 1, 1);
f = Phys3DHelper.GetKMuForce(me, toThis, 10, 10, 2 * diag);
Assert.AreEqual(diag * 10, f.GetLength(), 0.00001);
Assert.AreEqual(-10, f.X, 0.00001);
Assert.AreEqual(-10, f.Y, 0.00001);
Assert.AreEqual(-10, f.Z, 0.00001);
}
public void GetlocalTauForceTest()
{
var wheel = new RbWheel(
n: 7,
R: 10,
R_max: 15,
H_wheel: 1,
H_zac: 1,
kR: 77,
muR: 77,
kH: 66,
muH: 66);
wheel.Betta = 30 * PI / 180;
var f = wheel.GetlocalTauForce(new Vector3D(0.4, -10, 0));
var yansw = (0.2673 * 10 / 47.8570) * 66;
var zansw = -(3.5664 * 10 / 47.8570) * 66;
Assert.IsTrue(Vector3D.ApproxEqual(f, new Vector3D(0, yansw, zansw), 0.1));
}
