public BladeSet(Propeller parent, Types type, double radius, bool reversed, double speedRatio, double angleOffset) : base(parent, type, radius, reversed, speedRatio, angleOffset)
{
Kernel.Heart.Beat1 += (secs) =>
{
MyLib.Set(SubTransforms, TransformIndexTheta, false).RotatePrepend(new Vector3D(0, 0, 1), (Reversed ? -1 : 1) * (secs * SpeedRatio * 5 * Parent.omega)).Done();
UpdateTransform();
};
}
public Drill(double radius, double bladeCount) : base(radius, bladeCount)
{
SubTransforms.Add(new MatrixTransform3D()); //Withdraw
SubTransforms.Add(new MatrixTransform3D()); //Rotate
Kernel.Heart.Beat1 += (secs) =>
{
MyLib.Set(SubTransforms, TransformIndexRotate).RotatePrepend(new Vector3D(0, 0, 1), secs * Math.PI).Done();
UpdateTransform();
};
}
public Body() : base()
{
propeller.IsOnGround = () => IsOnGround();
Kernel.Heart.Beat1 += (secs) =>
{
//if (IsOnGround()) System.Diagnostics.Trace.WriteLine("A");
MaintainRotationY(secs);
MaintainRotationZ(secs);
//drill.Folding = System.Math.Abs(System.DateTime.Now.Ticks % 100000000 - 50000000) / 50000000.0;
{
RB.position.Z = RB.velocity.Z = 0;
RB.force = new Vector3D();
RigidBodyUpdating?.Invoke(secs, RB);
RB.force += new Vector3D(0, -RB.mass * Constants.Gravity, 0);
RB.force += new Vector3D(-Math.Sin(RB.theta) * propeller.LiftForce(), Math.Cos(RB.theta) * propeller.LiftForce(), 0);
NextRB = new RigidBody(RB);
MaintainRigidBody(secs);
}
{
double frictionAcceleration = (TrackSpeed > 0 ? -1 : 1) * (TrackFriction /*+ 0.5 * Math.Abs(TrackSpeed)*/);
double acceleration = 0;
if (IsOnGround() && Keyboard.IsDown(System.Windows.Input.Key.A, System.Windows.Input.Key.D))
{
if (!Keyboard.IsDown(RotationY > Math.PI / 2 ? System.Windows.Input.Key.D : System.Windows.Input.Key.A))
{
acceleration += Math.Min(MaxTrackAcceleration, MaxTrackPower / Math.Max(Math.Abs(TrackSpeed), double.MinValue));
}
else
{
frictionAcceleration += (TrackSpeed > 0 ? -1 : 1) * MaxTrackAcceleration;
}
}
NextTrackSpeed = TrackSpeed;
NextTrackSpeed += acceleration * secs;
if ((NextTrackSpeed > 0) != (NextTrackSpeed + frictionAcceleration * secs > 0))
{
NextTrackSpeed = 0;
}
else
{
NextTrackSpeed += frictionAcceleration * secs;
}
}
const double lookOffset = 1.3;
MyLib.SmoothTo(ref Kernel.CameraProperties.position, RB.position + new Vector3D(0, 0, 30 / Math.Pow(0.4 + SetZ(RB.position - Kernel.CameraProperties.position, 0).Length * 0.1, 0.5)), secs, 0.2);
var target = RB.position + 0.1 * RB.velocity - Kernel.CameraProperties.position;
target /= Math.Abs(target.Z);
var len = Math.Sqrt(Math.Pow(target.X, 2) + Math.Pow(target.Y, 2));
var targetLen = Math.Min(len, lookOffset);
target.X *= targetLen / len;
target.Y *= targetLen / len;
MyLib.SmoothTo(ref Kernel.CameraProperties.lookDirection, target, secs, 0.2);
//this.Folding = 1;
//for(int i=0;i<1;i++)
{
var color = (byte)MyLib.Rand.Next(200, 210);
var position = RB.position + new Vector3D(-Math.Cos(RotationY) * 1, 1, MyLib.Rand.NextDouble() / 2 + 0.5);
var speed = new Vector3D(2 * -Math.Cos(RotationY) + MyLib.Rand.NextDouble() - 0.5, 5 + MyLib.Rand.NextDouble() - 0.5, 0);
Fumes.Instance.AddFums(position, speed
, Color.FromArgb(50, color, color, color)
, 0.1, 2, 2, speed.Length * 1.5);
}
};
Kernel.Heart.Beat2 += () =>
{
RotationY = NextRotationY;
MyLib.Set(SubTransforms, TransformIndexRotateAroundY, true).RotatePrepend(new Vector3D(0, -1, 0), RotationY).Done();
MyLib.Set(SubTransforms, TransformIndexRotateAroundZ, true).RotatePrepend(new Vector3D(0, 0, 1), RB.theta).Done();
RB = NextRB;
TrackSpeed = NextTrackSpeed;
OriginTransform = MyLib.Transform(new MatrixTransform3D()).Translate(RB.position - new Point3D()).Value;
UpdateTransform();
};
}
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();
};
}
