public static bool IsCollidable(Point3D p, out int x, out int y)
{
x = (int)Math.Floor((p.X - Blocks.Anchor.X) / Blocks.Width);
y = (int)Math.Floor((p.Y - Blocks.Anchor.Y) / Blocks.Height);
return(Blocks.IsCollidable(x, y));
}
void MaintainRigidBody(double secs)
{
if (!NextRB.Update(secs, rb =>
{
if (secs > 1e-3)
{
return(false);
}
var dif = (rb.position - rb._position).Length;
if (dif > 0.5)
{
return(false);
}
int cur_x, cur_y, x = 0, y = 0;
Blocks.IsCollidable(rb.position, out cur_x, out cur_y);
if (secs < 1e-4)//accurate enough
{
for (int cpi = -1; ;)
{
cpi = this.CollidePoints.FindIndex(cpi + 1, p => Blocks.IsCollidable(rb.position + new Vector3D(Math.Cos(rb.theta) * p.X - Math.Sin(rb.theta) * p.Y, Math.Cos(rb.theta) * p.Y + Math.Sin(rb.theta) * p.X, p.Z), out x, out y));
if (cpi == -1)
{
//{
// if (!tracks.UpdateRigidBody(secs, Math.Cos(RotationY), RB.velocity - RB._velocity, RB.theta, out Vector3D reactionForce, out double reactionTorque)) return false;
// //RB.force += reactionForce;
// //RB.alpha += reactionTorque;
//}
return(true);
}
var cp = this.CollidePoints[cpi];
const double bounce = 0.5;
double t;
t = cp.X * -Math.Sin(rb.theta) + cp.Y * -Math.Cos(rb.theta);
if (x == cur_x - 1 && !Blocks.IsCollidable(cur_x, y) && rb.velocity.X + t * rb.omega < 0)//collide left, +x force. t=(cp.x*-Sin(theta)+cp.y*-Cos(theta)), (v.x+f/m) + (omega+f*t/I)*t = -b*(v.x + omega*t), f*(1/m+t^2/I)=(-b-1)*(v.x+omega*t)
{
var f = (-bounce - 1) * (rb.velocity.X + rb.omega * t) / (1.0 / rb.mass + t * t / rb.momentOfInertia);
//f += -rb.force.X * secs;
rb.velocity.X += f / rb.mass;
rb.omega += f * t / rb.momentOfInertia;
}
t = cp.X * -Math.Sin(rb.theta) + cp.Y * -Math.Cos(rb.theta);
if (x == cur_x + 1 && !Blocks.IsCollidable(cur_x, y) && rb.velocity.X + t * rb.omega > 0)//collide right, -x force. t=(cp.x*-Sin(theta)+cp.y*-Cos(theta)), (v.x-f/m) + (omega-f*t/I)*t = -b*(v.x + omega*t), f*(-1/m-t^2/I)=(-b-1)*(v.x+omega*t)
{
var f = (-bounce - 1) * (rb.velocity.X + rb.omega * t) / (-1.0 / rb.mass - t * t / rb.momentOfInertia);
//f += rb.force.X * secs;
rb.velocity.X -= f / rb.mass;
rb.omega -= f * t / rb.momentOfInertia;
}
t = cp.Y * -Math.Sin(rb.theta) + cp.X * Math.Cos(rb.theta);
if (y == cur_y - 1 && !Blocks.IsCollidable(x, cur_y) && rb.velocity.Y + t * rb.omega < 0)//collide down, +y force. t=(cp.y*-Sin(theta)+cp.x*Cos(theta)), (v.y+f/m) + (omega+f*t/I)*t = -b*(v.y + omega*t), f*(1/m+t^2/I)=(-b-1)*(v.y+omega*t)
{
var f = (-bounce - 1) * (rb.velocity.Y + rb.omega * t) / (1.0 / rb.mass + t * t / rb.momentOfInertia);
//f -= rb.force.Y * secs;
rb.velocity.Y += f / rb.mass;
rb.omega += f * t / rb.momentOfInertia;
}
t = cp.Y * -Math.Sin(rb.theta) + cp.X * Math.Cos(rb.theta);
if (y == cur_y + 1 && !Blocks.IsCollidable(x, cur_y) && rb.velocity.Y + t * rb.omega > 0)//collide up, -y force. t=(cp.y*-Sin(theta)+cp.x*Cos(theta)), (v.y-f/m) + (omega-f*t/I)*t = -b*(v.y + omega*t), f*(-1/m-t^2/I)=(-b-1)*(v.y+omega*t)
{
var f = (-bounce - 1) * (rb.velocity.Y + rb.omega * t) / (-1.0 / rb.mass - t * t / rb.momentOfInertia);
//f += rb.force.Y * secs;
rb.velocity.Y -= f / rb.mass;
rb.omega -= f * t / rb.momentOfInertia;
}
//System.Diagnostics.Trace.WriteLine($"position: {rb.position}, \tvelocity: {rb.velocity}, \ttheta: {rb.theta}, \tomega: {rb.omega}");
}
}
if (this.CollidePoints.Any(p => Blocks.IsCollidable((new Point3D() + p) * MatrixTZ, out x, out y)))
{
return(false);
}
//{
// if (!tracks.UpdateRigidBody(secs, Math.Cos(RotationY), RB.velocity - RB._velocity, RB.theta, out Vector3D reactionForce, out double reactionTorque)) return false;
// //RB.force += reactionForce;
// //RB.alpha += reactionTorque;
//}
return(true);
}))
{
MaintainRigidBody(0.5 * secs);
MaintainRigidBody(0.5 * secs);
}
}
void UpdateRigitBody(double secs, Vector3D parentVelocity)
{
if (!RB.Update(secs, rb =>
{
if (secs > 1e-3)
{
return(false);
}
var dif = (rb.position - rb._position).Length;
if (dif > Radius / 5)
{
return(false);
}
if (!Blocks.IsCollidable(rb.position, out int cur_x, out int cur_y))
{
//bool rollback = false;
const double rollbackSpeed = 0.1;
const double bounce = 0.1;
{
int x = 0, y = 0;
Point3D cp;
cp = rb.position + new Vector3D(-Radius, 0, 0);
if (Blocks.IsCollidable(cp, out x, out y))
{
if (x == cur_x - 1 && !Blocks.IsCollidable(cur_x, y) && rb.velocity.X < 0)//collide left, +x force. t=(cp.x*-Sin(theta)+cp.y*-Cos(theta)), (v.x+f/m) + (omega+f*t/I)*t = -b*(v.x + omega*t), f*(1/m+t^2/I)=(-b-1)*(v.x+omega*t)
{
var f = (-bounce - 1) * (rb.velocity.X) / (1.0 / rb.mass);
rb.velocity.X += f / rb.mass;
//rollback = true;
rb.position.X += secs * rollbackSpeed;
}
}
cp = rb.position + new Vector3D(Radius, 0, 0);
if (Blocks.IsCollidable(cp, out x, out y))
{
if (x == cur_x + 1 && !Blocks.IsCollidable(cur_x, y) && rb.velocity.X > 0)//collide right, -x force. t=(cp.x*-Sin(theta)+cp.y*-Cos(theta)), (v.x-f/m) + (omega-f*t/I)*t = -b*(v.x + omega*t), f*(-1/m-t^2/I)=(-b-1)*(v.x+omega*t)
{
var f = (-bounce - 1) * (rb.velocity.X) / (-1.0 / rb.mass);
rb.velocity.X -= f / rb.mass;
//rollback = true;
rb.position.X -= secs * rollbackSpeed;
}
}
cp = rb.position + new Vector3D(0, -Radius, 0);
if (Blocks.IsCollidable(cp, out x, out y))
{
if (y == cur_y - 1 && !Blocks.IsCollidable(x, cur_y) && rb.velocity.Y < 0)//collide down, +y force. t=(cp.y*-Sin(theta)+cp.x*Cos(theta)), (v.y+f/m) + (omega+f*t/I)*t = -b*(v.y + omega*t), f*(1/m+t^2/I)=(-b-1)*(v.y+omega*t)
{
var f = (-bounce - 1) * (rb.velocity.Y) / (1.0 / rb.mass);
rb.velocity.Y += f / rb.mass;
//rollback = true;
rb.position.Y += secs * rollbackSpeed;
onGround = 0.1;
}
}
cp = rb.position + new Vector3D(0, Radius, 0);
if (Blocks.IsCollidable(cp, out x, out y))
{
if (y == cur_y + 1 && !Blocks.IsCollidable(x, cur_y) && rb.velocity.Y > 0)//collide up, -y force. t=(cp.y*-Sin(theta)+cp.x*Cos(theta)), (v.y-f/m) + (omega-f*t/I)*t = -b*(v.y + omega*t), f*(-1/m-t^2/I)=(-b-1)*(v.y+omega*t)
{
var f = (-bounce - 1) * (rb.velocity.Y) / (-1.0 / rb.mass);
rb.velocity.Y -= f / rb.mass;
//rollback = true;
rb.position.Y -= secs * rollbackSpeed;
}
}
}
//if(false)
{
Blocks.IsCollidable(rb.position + new Vector3D(Blocks.Width / 2, Blocks.Height / 2, 0), out int cross_x, out int cross_y);
var vectorToCross = new Vector3D((Blocks.Anchor.X + Blocks.Width * cross_x) - rb.position.X, (Blocks.Anchor.Y + Blocks.Height * cross_y) - rb.position.Y, 0);
if (0 < vectorToCross.Length && vectorToCross.Length < Radius && Vector3D.DotProduct(rb.velocity, vectorToCross) > 0)
{
int x = vectorToCross.X < 0 ? cross_x - 1 : cross_x;
int y = vectorToCross.Y < 0 ? cross_y - 1 : cross_y;
if (Blocks.IsCollidable(x, y))
{
///(v+a*c)。c=-b*(v。c)
///(vx+a*cx)*cx+(vy+a*cy)*cy=-b*(vx*cx+vy*cy)
///a(cx*cx+cy*cy)+vx*cx+vy*cy=-b*(vx*cx+vy*cy)
///a=(-b*(vx*cx+vy*cy)-(vx*cx+vy*cy))/(cx*cx+cy*cy)
double a = ((-bounce - 1) * Vector3D.DotProduct(rb.velocity, vectorToCross)) / Vector3D.DotProduct(vectorToCross, vectorToCross);
rb.velocity += a * vectorToCross;
//rollback = true;
rb.position += -vectorToCross / vectorToCross.Length * secs * rollbackSpeed;
}
}
}
//if (rollback) rb.position = rb._position;
//System.Diagnostics.Trace.WriteLine($"position: {rb.position}, \tvelocity: {rb.velocity}, \ttheta: {rb.theta}, \tomega: {rb.omega}");
}
return(true);
}))