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(); }; }