public Gear(Point3D desiredPosition, double radius, double suspensionHardness, double mass, Body parent) : base(desiredPosition, radius, suspensionHardness, mass, parent)
{
var preMatrixY = Parent.MatrixY;
var preMatrixZ = Parent.MatrixZ;
Parent.RigidBodyUpdating += (secs, rb) =>
{
{
var newPosition = RB.position * MyLib.Inverse(preMatrixY * Parent.MatrixZ * Parent.MatrixT) * Parent.MatrixY * Parent.MatrixZ * Parent.MatrixT;
var newVelocity = RB.velocity * MyLib.Inverse(preMatrixY * Parent.MatrixZ * Parent.MatrixT) * Parent.MatrixY * Parent.MatrixZ * Parent.MatrixT;
//RB.position.X = newPosition.X; RB.position.Z = newPosition.Z;
RB.position = newPosition;
//RB.velocity.X = newVelocity.X; RB.velocity.Y = newVelocity.Y;
preMatrixY = Parent.MatrixY;
preMatrixZ = Parent.MatrixZ;
}
var parentVelocity = rb.GetVelocityAt(RelativePosition * Parent.MatrixY);
var f1 = ReactForce;
var f2 = -0.5 * (parentVelocity - RB.velocity);
//if (Math.Abs((new Vector3D(1, 0, 0) * Parent.MatrixY).Z) > 0.01)
//{
// //RB.velocity = parentVelocity;
// f1 *= 2;
// f2 *= 2;
//}
var f = f1 + f2;
rb.force += f;
onGround -= secs;
if (onGround < 0)
{
onGround = 0;
}
RB.force = new Vector3D(0, -RB.mass * Constants.Gravity, 0);
RB.force -= f;
{
var friction = (new Vector3D(Parent.TrackSpeed, 0, 0) * Parent.MatrixY - RB.velocity).X * 0.5 * Math.Max(0, ReactForce.Y);
double maxFriction = SuspensionHardness * 0.1;
if (friction > maxFriction)
{
friction = maxFriction;
}
if (friction < -maxFriction)
{
friction = -maxFriction;
}
RB.force.X += friction;
}
RB.theta += secs * Parent.TrackSpeed / Radius;
UpdateRigitBody(secs, parentVelocity);
///minimize: (px+a*fx)^2+(py+a*fy)^2
///2(px+a*fx)*fx+2(py+a*fy)*fy=0
///px*fx+a*fx*fx+py*fy+a*fy*fy=0
///a=-(px*fx+py*fy)/(fx*fx+fy*fy)
if (f.Length > 0)
{
var p = DesiredPosition - rb.position;
var a = -(p.X * f.X + p.Y * f.Y) / (f.X * f.X + f.Y * f.Y);
var forceArm = p + f * a;
var torque = Vector3D.CrossProduct(forceArm, f).Z;
//System.Diagnostics.Trace.WriteLine($"torque: {torque}");
rb.alpha += torque;
}
MyLib.Set(SubTransforms, TransformIndexPosition, true).TranslatePrepend(Position - new Point3D()).Done();
SubTransforms[TransformIndexRotateY] = new MatrixTransform3D(Parent.MatrixY);
MyLib.Set(SubTransforms, TransformIndexSpin, true).RotatePrepend(new Vector3D(0, 0, -1), RB.theta).Done();
UpdateTransform();
};
}