Antes de cambiar Motor de Fisica
This commit is contained in:
parent
2cb90ec2dc
commit
d259f53081
|
@ -0,0 +1,937 @@
|
||||||
|
using System;
|
||||||
|
using System.Collections.Generic;
|
||||||
|
using System.Diagnostics;
|
||||||
|
using System.Linq;
|
||||||
|
using System.Windows;
|
||||||
|
using System.Windows.Controls;
|
||||||
|
using System.Windows.Media;
|
||||||
|
using System.Windows.Shapes;
|
||||||
|
using BulletSharp;
|
||||||
|
using BulletSharp.Math;
|
||||||
|
|
||||||
|
namespace CtrEditor.Simulacion
|
||||||
|
{
|
||||||
|
public class simBase
|
||||||
|
{
|
||||||
|
public RigidBody Body { get; protected set; }
|
||||||
|
public DiscreteDynamicsWorld _world;
|
||||||
|
|
||||||
|
public void RemoverBody()
|
||||||
|
{
|
||||||
|
if (Body != null && _world != null)
|
||||||
|
{
|
||||||
|
_world.RemoveRigidBody(Body);
|
||||||
|
Body?.Dispose();
|
||||||
|
Body = null;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
public static float Min(float Value, float Min = 0.01f)
|
||||||
|
{
|
||||||
|
return Value < Min ? Min : Value;
|
||||||
|
}
|
||||||
|
|
||||||
|
public static float GradosARadianes(float grados)
|
||||||
|
{
|
||||||
|
return (float)(grados * (Math.PI / 180));
|
||||||
|
}
|
||||||
|
|
||||||
|
public static float RadianesAGrados(float radianes)
|
||||||
|
{
|
||||||
|
return (float)(radianes * (180 / Math.PI));
|
||||||
|
}
|
||||||
|
|
||||||
|
public void SetPosition(float x, float y)
|
||||||
|
{
|
||||||
|
if (Body != null)
|
||||||
|
{
|
||||||
|
var transform = Body.WorldTransform;
|
||||||
|
transform.Origin = new Vector3(x, y, 0);
|
||||||
|
Body.WorldTransform = transform;
|
||||||
|
Body.Activate();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
public void SetPosition(Vector3 centro)
|
||||||
|
{
|
||||||
|
try
|
||||||
|
{
|
||||||
|
if (Body != null)
|
||||||
|
{
|
||||||
|
var transform = Body.WorldTransform;
|
||||||
|
transform.Origin = centro;
|
||||||
|
Body.WorldTransform = transform;
|
||||||
|
Body.Activate();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
catch
|
||||||
|
{
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
public Vector3 GetPosition()
|
||||||
|
{
|
||||||
|
return Body?.WorldTransform.Origin ?? Vector3.Zero;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
public class simCurve : simBase
|
||||||
|
{
|
||||||
|
private float _innerRadius;
|
||||||
|
private float _outerRadius;
|
||||||
|
private float _startAngle;
|
||||||
|
private float _endAngle;
|
||||||
|
public float Speed { get; set; }
|
||||||
|
|
||||||
|
private List<Action> _deferredActions;
|
||||||
|
|
||||||
|
public simCurve(DiscreteDynamicsWorld world, List<Action> deferredActions, float innerRadius, float outerRadius, float startAngle, float endAngle, Vector3 position)
|
||||||
|
{
|
||||||
|
_world = world;
|
||||||
|
_deferredActions = deferredActions;
|
||||||
|
_innerRadius = innerRadius;
|
||||||
|
_outerRadius = outerRadius;
|
||||||
|
_startAngle = GradosARadianes(startAngle);
|
||||||
|
_endAngle = GradosARadianes(endAngle);
|
||||||
|
Create(position);
|
||||||
|
}
|
||||||
|
|
||||||
|
public void Create(float innerRadius, float outerRadius, float startAngle, float endAngle, Vector3 position)
|
||||||
|
{
|
||||||
|
if (_world == null)
|
||||||
|
return;
|
||||||
|
_innerRadius = innerRadius;
|
||||||
|
_outerRadius = outerRadius;
|
||||||
|
_startAngle = GradosARadianes(startAngle);
|
||||||
|
_endAngle = GradosARadianes(endAngle);
|
||||||
|
Create(position);
|
||||||
|
}
|
||||||
|
|
||||||
|
public void Create(Vector3 position)
|
||||||
|
{
|
||||||
|
RemoverBody();
|
||||||
|
|
||||||
|
// Crear una forma compuesta para la curva
|
||||||
|
var compoundShape = new CompoundShape();
|
||||||
|
|
||||||
|
// Crear segmentos de la curva como cajas pequeñas
|
||||||
|
int segments = 32;
|
||||||
|
float angleStep = (_endAngle - _startAngle) / segments;
|
||||||
|
float thickness = 0.1f; // Grosor de la curva
|
||||||
|
|
||||||
|
for (int i = 0; i < segments; i++)
|
||||||
|
{
|
||||||
|
float angle = _startAngle + i * angleStep;
|
||||||
|
float midRadius = (_innerRadius + _outerRadius) / 2f;
|
||||||
|
float segmentWidth = (_outerRadius - _innerRadius);
|
||||||
|
float segmentLength = midRadius * angleStep;
|
||||||
|
|
||||||
|
var segmentShape = new BoxShape(segmentLength / 2f, segmentWidth / 2f, thickness / 2f);
|
||||||
|
|
||||||
|
var segmentTransform = Matrix.Identity;
|
||||||
|
segmentTransform.Origin = new Vector3(
|
||||||
|
midRadius * (float)Math.Cos(angle),
|
||||||
|
midRadius * (float)Math.Sin(angle),
|
||||||
|
0
|
||||||
|
);
|
||||||
|
segmentTransform = Matrix.RotationZ(angle) * segmentTransform;
|
||||||
|
|
||||||
|
compoundShape.AddChildShape(segmentTransform, segmentShape);
|
||||||
|
}
|
||||||
|
|
||||||
|
var motionState = new DefaultMotionState(Matrix.Translation(position));
|
||||||
|
var rbInfo = new RigidBodyConstructionInfo(0, motionState, compoundShape, Vector3.Zero);
|
||||||
|
Body = new RigidBody(rbInfo);
|
||||||
|
Body.UserObject = this;
|
||||||
|
Body.CollisionFlags |= CollisionFlags.NoContactResponse; // Sensor
|
||||||
|
|
||||||
|
_world.AddRigidBody(Body);
|
||||||
|
}
|
||||||
|
|
||||||
|
public void SetSpeed(float speed)
|
||||||
|
{
|
||||||
|
Speed = speed;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void ApplyCurveEffect(RigidBody bottle)
|
||||||
|
{
|
||||||
|
Vector3 centerToBottle = bottle.WorldTransform.Origin - Body.WorldTransform.Origin;
|
||||||
|
float distanceToCenter = centerToBottle.Length;
|
||||||
|
|
||||||
|
if (distanceToCenter >= _innerRadius && distanceToCenter <= _outerRadius)
|
||||||
|
{
|
||||||
|
float overlapPercentage = CalculateAngleOverlap(bottle);
|
||||||
|
|
||||||
|
if (overlapPercentage > 0)
|
||||||
|
{
|
||||||
|
float speedMetersPerSecond = Speed / 60.0f;
|
||||||
|
Vector3 tangent = new Vector3(-centerToBottle.Y, centerToBottle.X, 0);
|
||||||
|
tangent.Normalize();
|
||||||
|
|
||||||
|
if (speedMetersPerSecond < 0)
|
||||||
|
tangent = -tangent;
|
||||||
|
|
||||||
|
Vector3 currentVelocity = bottle.LinearVelocity;
|
||||||
|
float currentSpeed = currentVelocity.Length;
|
||||||
|
float conveyorSpeed = Math.Abs(speedMetersPerSecond);
|
||||||
|
float targetSpeed = Math.Max(currentSpeed, conveyorSpeed);
|
||||||
|
|
||||||
|
Vector3 currentDir = currentVelocity;
|
||||||
|
if (currentDir.LengthSquared > 0)
|
||||||
|
currentDir.Normalize();
|
||||||
|
else
|
||||||
|
currentDir = tangent;
|
||||||
|
|
||||||
|
Vector3 newDirection = currentDir * (1 - overlapPercentage) + tangent * overlapPercentage;
|
||||||
|
newDirection.Normalize();
|
||||||
|
|
||||||
|
bottle.LinearVelocity = newDirection * targetSpeed;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
private float CalculateAngleOverlap(RigidBody bottle)
|
||||||
|
{
|
||||||
|
Vector3 centerToBottle = bottle.WorldTransform.Origin - Body.WorldTransform.Origin;
|
||||||
|
float bottleAngle = (float)Math.Atan2(centerToBottle.Y, centerToBottle.X);
|
||||||
|
|
||||||
|
float normalizedBottleAngle = bottleAngle < 0 ? bottleAngle + 2 * (float)Math.PI : bottleAngle;
|
||||||
|
float normalizedStartAngle = _startAngle < 0 ? _startAngle + 2 * (float)Math.PI : _startAngle;
|
||||||
|
float normalizedEndAngle = _endAngle < 0 ? _endAngle + 2 * (float)Math.PI : _endAngle;
|
||||||
|
|
||||||
|
if (normalizedStartAngle > normalizedEndAngle)
|
||||||
|
{
|
||||||
|
if (!(normalizedBottleAngle >= normalizedStartAngle || normalizedBottleAngle <= normalizedEndAngle))
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
if (normalizedBottleAngle < normalizedStartAngle || normalizedBottleAngle > normalizedEndAngle)
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
float distanceToCenter = centerToBottle.Length;
|
||||||
|
if (distanceToCenter < _innerRadius || distanceToCenter > _outerRadius)
|
||||||
|
return 0;
|
||||||
|
|
||||||
|
float angleFromStart, angleToEnd, totalAngle;
|
||||||
|
|
||||||
|
if (normalizedStartAngle > normalizedEndAngle)
|
||||||
|
{
|
||||||
|
totalAngle = (2 * (float)Math.PI - normalizedStartAngle) + normalizedEndAngle;
|
||||||
|
if (normalizedBottleAngle >= normalizedStartAngle)
|
||||||
|
{
|
||||||
|
angleFromStart = normalizedBottleAngle - normalizedStartAngle;
|
||||||
|
angleToEnd = (2 * (float)Math.PI - normalizedBottleAngle) + normalizedEndAngle;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
angleFromStart = (2 * (float)Math.PI - normalizedStartAngle) + normalizedBottleAngle;
|
||||||
|
angleToEnd = normalizedEndAngle - normalizedBottleAngle;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
angleFromStart = normalizedBottleAngle - normalizedStartAngle;
|
||||||
|
angleToEnd = normalizedEndAngle - normalizedBottleAngle;
|
||||||
|
totalAngle = normalizedEndAngle - normalizedStartAngle;
|
||||||
|
}
|
||||||
|
|
||||||
|
float minAngleDistance = Math.Min(angleFromStart, angleToEnd);
|
||||||
|
float overlapPercentage = Math.Min(minAngleDistance / (totalAngle * 0.1f), 1.0f);
|
||||||
|
return overlapPercentage;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
public class simDescarte : simBase
|
||||||
|
{
|
||||||
|
private float _radius;
|
||||||
|
private List<Action> _deferredActions;
|
||||||
|
|
||||||
|
public simDescarte(DiscreteDynamicsWorld world, List<Action> deferredActions, float diameter, Vector3 position)
|
||||||
|
{
|
||||||
|
_world = world;
|
||||||
|
_deferredActions = deferredActions;
|
||||||
|
_radius = diameter / 2;
|
||||||
|
Create(position);
|
||||||
|
}
|
||||||
|
|
||||||
|
public void SetDiameter(float diameter)
|
||||||
|
{
|
||||||
|
if (diameter <= 0)
|
||||||
|
diameter = 0.01f;
|
||||||
|
_radius = diameter / 2;
|
||||||
|
Create(Body.WorldTransform.Origin);
|
||||||
|
}
|
||||||
|
|
||||||
|
public void Create(Vector3 position)
|
||||||
|
{
|
||||||
|
RemoverBody();
|
||||||
|
|
||||||
|
var shape = new CylinderShape(_radius, _radius, 0.1f); // Cilindro plano para simular círculo 2D
|
||||||
|
var motionState = new DefaultMotionState(Matrix.Translation(position));
|
||||||
|
var rbInfo = new RigidBodyConstructionInfo(0, motionState, shape, Vector3.Zero);
|
||||||
|
Body = new RigidBody(rbInfo);
|
||||||
|
Body.UserObject = this;
|
||||||
|
Body.CollisionFlags |= CollisionFlags.NoContactResponse; // Sensor
|
||||||
|
|
||||||
|
_world.AddRigidBody(Body);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
public class simTransporte : simBase
|
||||||
|
{
|
||||||
|
public float Speed { get; set; }
|
||||||
|
public float Friction { get; set; }
|
||||||
|
public float DistanceGuide2Guide { get; set; }
|
||||||
|
public bool isBrake { get; set; }
|
||||||
|
public bool TransportWithGuides = false;
|
||||||
|
|
||||||
|
private List<Action> _deferredActions;
|
||||||
|
public float Width { get; set; }
|
||||||
|
public float Height { get; set; }
|
||||||
|
|
||||||
|
public simTransporte(DiscreteDynamicsWorld world, List<Action> deferredActions, float width, float height, Vector3 position, float angle = 0)
|
||||||
|
{
|
||||||
|
_world = world;
|
||||||
|
_deferredActions = deferredActions;
|
||||||
|
Create(width, height, position, angle);
|
||||||
|
}
|
||||||
|
|
||||||
|
public float Angle
|
||||||
|
{
|
||||||
|
get
|
||||||
|
{
|
||||||
|
var rotation = Body.WorldTransform.GetBasis();
|
||||||
|
return RadianesAGrados((float)Math.Atan2(rotation.M21, rotation.M11));
|
||||||
|
}
|
||||||
|
set
|
||||||
|
{
|
||||||
|
var transform = Body.WorldTransform;
|
||||||
|
transform.Basis = Matrix3x3.CreateRotationZ(GradosARadianes(value));
|
||||||
|
Body.WorldTransform = transform;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
public new void SetPosition(float x, float y)
|
||||||
|
{
|
||||||
|
var transform = Body.WorldTransform;
|
||||||
|
transform.Origin = new Vector3(x, y, 0);
|
||||||
|
Body.WorldTransform = transform;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void SetSpeed(float speed)
|
||||||
|
{
|
||||||
|
Speed = speed;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void SetDimensions(float width, float height)
|
||||||
|
{
|
||||||
|
RemoverBody();
|
||||||
|
Create(width, height, Body.WorldTransform.Origin, Angle);
|
||||||
|
}
|
||||||
|
|
||||||
|
public void Create(float width, float height, Vector3 position, float angle = 0)
|
||||||
|
{
|
||||||
|
RemoverBody();
|
||||||
|
Width = width;
|
||||||
|
Height = height;
|
||||||
|
Friction = 0.1f;
|
||||||
|
|
||||||
|
var shape = new BoxShape(width / 2, height / 2, 0.05f); // Caja plana para simular rectángulo 2D
|
||||||
|
var transform = Matrix.Translation(position) * Matrix.RotationZ(GradosARadianes(angle));
|
||||||
|
var motionState = new DefaultMotionState(transform);
|
||||||
|
var rbInfo = new RigidBodyConstructionInfo(0, motionState, shape, Vector3.Zero);
|
||||||
|
Body = new RigidBody(rbInfo);
|
||||||
|
Body.UserObject = this;
|
||||||
|
Body.CollisionFlags |= CollisionFlags.NoContactResponse; // Sensor
|
||||||
|
|
||||||
|
_world.AddRigidBody(Body);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
public class simBarrera : simBase
|
||||||
|
{
|
||||||
|
public float Distancia;
|
||||||
|
public int LuzCortada;
|
||||||
|
public bool LuzCortadaNeck;
|
||||||
|
public bool DetectNeck;
|
||||||
|
public List<simBotella> ListSimBotellaContact;
|
||||||
|
|
||||||
|
float _height;
|
||||||
|
private List<Action> _deferredActions;
|
||||||
|
|
||||||
|
public simBarrera(DiscreteDynamicsWorld world, List<Action> deferredActions, float width, float height, Vector3 position, float angle = 0, bool detectectNeck = false)
|
||||||
|
{
|
||||||
|
_world = world;
|
||||||
|
_height = Min(height);
|
||||||
|
DetectNeck = detectectNeck;
|
||||||
|
_deferredActions = deferredActions;
|
||||||
|
ListSimBotellaContact = new List<simBotella>();
|
||||||
|
Create(Min(width), _height, position, angle);
|
||||||
|
}
|
||||||
|
|
||||||
|
public float Angle
|
||||||
|
{
|
||||||
|
get
|
||||||
|
{
|
||||||
|
var rotation = Body.WorldTransform.GetBasis();
|
||||||
|
return RadianesAGrados((float)Math.Atan2(rotation.M21, rotation.M11));
|
||||||
|
}
|
||||||
|
set
|
||||||
|
{
|
||||||
|
var transform = Body.WorldTransform;
|
||||||
|
transform.Basis = Matrix3x3.CreateRotationZ(GradosARadianes(value));
|
||||||
|
Body.WorldTransform = transform;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
public new void SetPosition(float x, float y)
|
||||||
|
{
|
||||||
|
var transform = Body.WorldTransform;
|
||||||
|
transform.Origin = new Vector3(x, y, 0);
|
||||||
|
Body.WorldTransform = transform;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void SetDimensions(float width, float height)
|
||||||
|
{
|
||||||
|
RemoverBody();
|
||||||
|
Create(Min(width), Min(height), Body.WorldTransform.Origin, Angle);
|
||||||
|
}
|
||||||
|
|
||||||
|
public void Create(float width, float height, Vector3 position, float angle = 0, bool detectectNeck = false)
|
||||||
|
{
|
||||||
|
RemoverBody();
|
||||||
|
_height = Min(height);
|
||||||
|
DetectNeck = detectectNeck;
|
||||||
|
|
||||||
|
var shape = new BoxShape(Min(width) / 2, _height / 2, 0.05f);
|
||||||
|
var transform = Matrix.Translation(position) * Matrix.RotationZ(GradosARadianes(angle));
|
||||||
|
var motionState = new DefaultMotionState(transform);
|
||||||
|
var rbInfo = new RigidBodyConstructionInfo(0, motionState, shape, Vector3.Zero);
|
||||||
|
Body = new RigidBody(rbInfo);
|
||||||
|
Body.UserObject = this;
|
||||||
|
Body.CollisionFlags |= CollisionFlags.NoContactResponse; // Sensor
|
||||||
|
|
||||||
|
_world.AddRigidBody(Body);
|
||||||
|
LuzCortada = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void CheckIfNecksIsTouching()
|
||||||
|
{
|
||||||
|
if (LuzCortada == 0)
|
||||||
|
return;
|
||||||
|
|
||||||
|
foreach (var botella in ListSimBotellaContact)
|
||||||
|
{
|
||||||
|
Vector3 sensorCenter = Body.WorldTransform.Origin;
|
||||||
|
float halfHeight = _height / 2;
|
||||||
|
var rotation = Body.WorldTransform.GetBasis();
|
||||||
|
float cos = rotation.M11;
|
||||||
|
float sin = rotation.M21;
|
||||||
|
|
||||||
|
Vector3 lineStart = sensorCenter + new Vector3(-halfHeight * cos, halfHeight * sin, 0);
|
||||||
|
Vector3 lineEnd = sensorCenter + new Vector3(halfHeight * cos, -halfHeight * sin, 0);
|
||||||
|
|
||||||
|
Vector3 fixtureCenter = botella.Body.WorldTransform.Origin;
|
||||||
|
Vector3 closestPoint = ProjectPointOntoLine(fixtureCenter, lineStart, lineEnd);
|
||||||
|
|
||||||
|
float distance = (closestPoint - fixtureCenter).Length;
|
||||||
|
Distancia = distance;
|
||||||
|
|
||||||
|
if (distance <= botella._neckRadius)
|
||||||
|
{
|
||||||
|
LuzCortadaNeck = true;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
LuzCortadaNeck = false;
|
||||||
|
}
|
||||||
|
|
||||||
|
private Vector3 ProjectPointOntoLine(Vector3 point, Vector3 lineStart, Vector3 lineEnd)
|
||||||
|
{
|
||||||
|
Vector3 lineDirection = lineEnd - lineStart;
|
||||||
|
lineDirection.Normalize();
|
||||||
|
Vector3 pointToLineStart = point - lineStart;
|
||||||
|
float projectionLength = Vector3.Dot(pointToLineStart, lineDirection);
|
||||||
|
return lineStart + projectionLength * lineDirection;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
public class simGuia : simBase
|
||||||
|
{
|
||||||
|
private List<Action> _deferredActions;
|
||||||
|
|
||||||
|
public simGuia(DiscreteDynamicsWorld world, List<Action> deferredActions, Vector3 start, Vector3 end)
|
||||||
|
{
|
||||||
|
_world = world;
|
||||||
|
_deferredActions = deferredActions;
|
||||||
|
Create(start, end);
|
||||||
|
}
|
||||||
|
|
||||||
|
public void Create(Vector3 start, Vector3 end)
|
||||||
|
{
|
||||||
|
RemoverBody();
|
||||||
|
|
||||||
|
Vector3 center = (start + end) / 2;
|
||||||
|
Vector3 direction = end - start;
|
||||||
|
float length = direction.Length;
|
||||||
|
|
||||||
|
var shape = new BoxShape(length / 2, 0.01f, 0.05f); // Línea como caja muy delgada
|
||||||
|
|
||||||
|
float angle = (float)Math.Atan2(direction.Y, direction.X);
|
||||||
|
var transform = Matrix.Translation(center) * Matrix.RotationZ(angle);
|
||||||
|
var motionState = new DefaultMotionState(transform);
|
||||||
|
var rbInfo = new RigidBodyConstructionInfo(0, motionState, shape, Vector3.Zero);
|
||||||
|
Body = new RigidBody(rbInfo);
|
||||||
|
Body.UserObject = this;
|
||||||
|
|
||||||
|
_world.AddRigidBody(Body);
|
||||||
|
}
|
||||||
|
|
||||||
|
public void UpdateVertices(Vector3 newStart, Vector3 newEnd)
|
||||||
|
{
|
||||||
|
Create(newStart, newEnd);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
public class simBotella : simBase
|
||||||
|
{
|
||||||
|
public float Radius;
|
||||||
|
private float _mass;
|
||||||
|
public bool Descartar = false;
|
||||||
|
public int isOnTransports;
|
||||||
|
public List<simBase> ListOnTransports;
|
||||||
|
public bool isRestricted;
|
||||||
|
public bool isNoMoreRestricted;
|
||||||
|
public float OriginalMass;
|
||||||
|
public simTransporte ConveyorRestrictedTo;
|
||||||
|
public RigidBody axisRestrictedBy;
|
||||||
|
public float OverlapPercentage;
|
||||||
|
public float _neckRadius;
|
||||||
|
|
||||||
|
private List<Action> _deferredActions;
|
||||||
|
private DiscreteDynamicsWorld _worldRef;
|
||||||
|
|
||||||
|
public simBotella(DiscreteDynamicsWorld world, List<Action> deferredActions, float diameter, Vector3 position, float mass, float neckRadius = 0)
|
||||||
|
{
|
||||||
|
_world = world;
|
||||||
|
_worldRef = world;
|
||||||
|
ListOnTransports = new List<simBase>();
|
||||||
|
_deferredActions = deferredActions;
|
||||||
|
|
||||||
|
diameter = Min(diameter, 0.01f);
|
||||||
|
Radius = diameter / 2;
|
||||||
|
_mass = mass;
|
||||||
|
|
||||||
|
if (neckRadius <= 0)
|
||||||
|
neckRadius = diameter / 4;
|
||||||
|
_neckRadius = neckRadius;
|
||||||
|
Create(position);
|
||||||
|
}
|
||||||
|
|
||||||
|
public float CenterX => Body.WorldTransform.Origin.X;
|
||||||
|
public float CenterY => Body.WorldTransform.Origin.Y;
|
||||||
|
public Vector3 Center => Body.WorldTransform.Origin;
|
||||||
|
|
||||||
|
public float Mass
|
||||||
|
{
|
||||||
|
get
|
||||||
|
{
|
||||||
|
if (_mass <= 0)
|
||||||
|
_mass = 1;
|
||||||
|
return _mass;
|
||||||
|
}
|
||||||
|
set
|
||||||
|
{
|
||||||
|
_mass = value;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
private void Create(Vector3 position)
|
||||||
|
{
|
||||||
|
RemoverBody();
|
||||||
|
isOnTransports = 0;
|
||||||
|
|
||||||
|
var shape = new CylinderShape(Radius, Radius, 0.1f); // Cilindro para simular círculo 2D
|
||||||
|
var motionState = new DefaultMotionState(Matrix.Translation(position));
|
||||||
|
|
||||||
|
Vector3 inertia;
|
||||||
|
shape.CalculateLocalInertia(_mass, out inertia);
|
||||||
|
var rbInfo = new RigidBodyConstructionInfo(_mass, motionState, shape, inertia);
|
||||||
|
|
||||||
|
Body = new RigidBody(rbInfo);
|
||||||
|
Body.UserObject = this;
|
||||||
|
Body.SetDamping(3f, 1f); // Linear y angular damping
|
||||||
|
Body.SetSleepingThresholds(0, 0); // No dormir
|
||||||
|
Body.CcdMotionThreshold = 1e-7f;
|
||||||
|
Body.CcdSweptSphereRadius = Radius * 0.2f;
|
||||||
|
|
||||||
|
_world.AddRigidBody(Body);
|
||||||
|
}
|
||||||
|
|
||||||
|
public void SetDiameter(float diameter)
|
||||||
|
{
|
||||||
|
diameter = Min(diameter, 0.01f);
|
||||||
|
Radius = diameter / 2;
|
||||||
|
Create(Body.WorldTransform.Origin);
|
||||||
|
}
|
||||||
|
|
||||||
|
public void SetMass(float mass)
|
||||||
|
{
|
||||||
|
Mass = mass;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void ApplyConveyorSpeed(float deltaTime_s)
|
||||||
|
{
|
||||||
|
foreach (var transporte in ListOnTransports)
|
||||||
|
{
|
||||||
|
if (transporte is simTransporte conveyorRect)
|
||||||
|
ApplyConveyorEffect(deltaTime_s, conveyorRect);
|
||||||
|
if (transporte is simCurve conveyorCurve)
|
||||||
|
conveyorCurve.ApplyCurveEffect(Body);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
private bool ApplyConveyorEffect(float deltaTime_s, simTransporte conveyor)
|
||||||
|
{
|
||||||
|
float overlapPercentage = CalculateOverlapedArea(this, conveyor);
|
||||||
|
OverlapPercentage = overlapPercentage;
|
||||||
|
|
||||||
|
float speedMetersPerSecond = conveyor.Speed / 60.0f;
|
||||||
|
var rotation = conveyor.Body.WorldTransform.GetBasis();
|
||||||
|
Vector3 desiredVelocity = new Vector3(rotation.M11, rotation.M21, 0) * speedMetersPerSecond;
|
||||||
|
|
||||||
|
Vector3 currentVelocity = Body.LinearVelocity;
|
||||||
|
Vector3 velocityDifference = desiredVelocity - currentVelocity;
|
||||||
|
|
||||||
|
float proporcionalVelocityNeeded = 1 - CalculateProportion(currentVelocity, desiredVelocity);
|
||||||
|
float frictionCoefficient;
|
||||||
|
|
||||||
|
switch (proporcionalVelocityNeeded)
|
||||||
|
{
|
||||||
|
case > 0.3f:
|
||||||
|
frictionCoefficient = conveyor.Friction * overlapPercentage;
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
frictionCoefficient = proporcionalVelocityNeeded * overlapPercentage;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (isRestricted && conveyor == ConveyorRestrictedTo && overlapPercentage > 0.5f)
|
||||||
|
{
|
||||||
|
Body.LinearVelocity = desiredVelocity;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
Body.LinearVelocity += frictionCoefficient * desiredVelocity;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
public float CalculateOverlapedArea(simBotella botella, simTransporte conveyor)
|
||||||
|
{
|
||||||
|
// Implementación simplificada del cálculo de área superpuesta
|
||||||
|
Vector3 bottlePos = botella.Body.WorldTransform.Origin;
|
||||||
|
Vector3 conveyorPos = conveyor.Body.WorldTransform.Origin;
|
||||||
|
|
||||||
|
// Transformar posición de botella al espacio local del transportador
|
||||||
|
Matrix invTransform = conveyor.Body.WorldTransform.Inverse();
|
||||||
|
Vector3 localBottlePos = Vector3.TransformCoordinate(bottlePos, invTransform);
|
||||||
|
|
||||||
|
// Verificar si la botella está dentro del rectángulo del transportador
|
||||||
|
float halfWidth = conveyor.Width / 2;
|
||||||
|
float halfHeight = conveyor.Height / 2;
|
||||||
|
|
||||||
|
if (Math.Abs(localBottlePos.X) <= halfWidth + botella.Radius &&
|
||||||
|
Math.Abs(localBottlePos.Y) <= halfHeight + botella.Radius)
|
||||||
|
{
|
||||||
|
// Calcular área superpuesta aproximada
|
||||||
|
float overlapX = Math.Max(0, Math.Min(halfWidth, localBottlePos.X + botella.Radius) - Math.Max(-halfWidth, localBottlePos.X - botella.Radius));
|
||||||
|
float overlapY = Math.Max(0, Math.Min(halfHeight, localBottlePos.Y + botella.Radius) - Math.Max(-halfHeight, localBottlePos.Y - botella.Radius));
|
||||||
|
|
||||||
|
float overlapArea = overlapX * overlapY;
|
||||||
|
float bottleArea = (float)(Math.PI * botella.Radius * botella.Radius);
|
||||||
|
|
||||||
|
return Math.Min(1.0f, overlapArea / bottleArea);
|
||||||
|
}
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
public static float CalculateProportion(Vector3 currentVelocity, Vector3 desiredVelocity)
|
||||||
|
{
|
||||||
|
float dotProduct = Vector3.Dot(desiredVelocity, currentVelocity);
|
||||||
|
float magnitudeV1Squared = desiredVelocity.LengthSquared;
|
||||||
|
|
||||||
|
if (magnitudeV1Squared == 0)
|
||||||
|
return 0;
|
||||||
|
|
||||||
|
float proportion = dotProduct / magnitudeV1Squared;
|
||||||
|
return proportion;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void CenterFixtureOnConveyor()
|
||||||
|
{
|
||||||
|
if (!isRestricted || ConveyorRestrictedTo == null)
|
||||||
|
return;
|
||||||
|
|
||||||
|
Vector3 conveyorCenter = ConveyorRestrictedTo.Body.WorldTransform.Origin;
|
||||||
|
float halfDistance = ConveyorRestrictedTo.DistanceGuide2Guide / 2;
|
||||||
|
var rotation = ConveyorRestrictedTo.Body.WorldTransform.GetBasis();
|
||||||
|
|
||||||
|
Vector3 offset = new Vector3(halfDistance * rotation.M11, halfDistance * rotation.M21, 0);
|
||||||
|
Vector3 lineStart = conveyorCenter - offset;
|
||||||
|
Vector3 lineEnd = conveyorCenter + offset;
|
||||||
|
|
||||||
|
Vector3 fixtureCenter = Body.WorldTransform.Origin;
|
||||||
|
Vector3 closestPoint = ProjectPointOntoLine(fixtureCenter, lineStart, lineEnd);
|
||||||
|
|
||||||
|
var transform = Body.WorldTransform;
|
||||||
|
transform.Origin = closestPoint;
|
||||||
|
Body.WorldTransform = transform;
|
||||||
|
}
|
||||||
|
|
||||||
|
private Vector3 ProjectPointOntoLine(Vector3 point, Vector3 lineStart, Vector3 lineEnd)
|
||||||
|
{
|
||||||
|
Vector3 lineDirection = lineEnd - lineStart;
|
||||||
|
lineDirection.Normalize();
|
||||||
|
Vector3 pointToLineStart = point - lineStart;
|
||||||
|
float projectionLength = Vector3.Dot(pointToLineStart, lineDirection);
|
||||||
|
return lineStart + projectionLength * lineDirection;
|
||||||
|
}
|
||||||
|
|
||||||
|
public bool IsOnAnyTransport()
|
||||||
|
{
|
||||||
|
return isOnTransports > 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
public class SimulationManagerFP
|
||||||
|
{
|
||||||
|
private DiscreteDynamicsWorld world;
|
||||||
|
private Canvas simulationCanvas;
|
||||||
|
public List<simBase> Cuerpos;
|
||||||
|
public List<Action> _deferredActions;
|
||||||
|
private Stopwatch stopwatch;
|
||||||
|
private double stopwatch_last;
|
||||||
|
|
||||||
|
// Componentes de Bullet Physics
|
||||||
|
private DefaultCollisionConfiguration collisionConfiguration;
|
||||||
|
private CollisionDispatcher dispatcher;
|
||||||
|
private DbvtBroadphase broadphase;
|
||||||
|
private SequentialImpulseConstraintSolver solver;
|
||||||
|
|
||||||
|
public Canvas DebugCanvas { get => simulationCanvas; set => simulationCanvas = value; }
|
||||||
|
|
||||||
|
public SimulationManagerFP()
|
||||||
|
{
|
||||||
|
// Inicializar Bullet Physics
|
||||||
|
collisionConfiguration = new DefaultCollisionConfiguration();
|
||||||
|
dispatcher = new CollisionDispatcher(collisionConfiguration);
|
||||||
|
broadphase = new DbvtBroadphase();
|
||||||
|
solver = new SequentialImpulseConstraintSolver();
|
||||||
|
|
||||||
|
world = new DiscreteDynamicsWorld(dispatcher, broadphase, solver, collisionConfiguration);
|
||||||
|
world.Gravity = new Vector3(0, 0, 0); // Sin gravedad para simular 2D
|
||||||
|
|
||||||
|
Cuerpos = new List<simBase>();
|
||||||
|
_deferredActions = new List<Action>();
|
||||||
|
stopwatch = new Stopwatch();
|
||||||
|
stopwatch.Start();
|
||||||
|
}
|
||||||
|
|
||||||
|
public void Clear()
|
||||||
|
{
|
||||||
|
if (world.NumCollisionObjects > 0)
|
||||||
|
{
|
||||||
|
for (int i = world.NumCollisionObjects - 1; i >= 0; i--)
|
||||||
|
{
|
||||||
|
var obj = world.CollisionObjectArray[i];
|
||||||
|
var body = RigidBody.Upcast(obj);
|
||||||
|
if (body != null && body.MotionState != null)
|
||||||
|
{
|
||||||
|
body.MotionState.Dispose();
|
||||||
|
}
|
||||||
|
world.RemoveCollisionObject(obj);
|
||||||
|
obj.Dispose();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (Cuerpos.Count > 0)
|
||||||
|
Cuerpos.Clear();
|
||||||
|
}
|
||||||
|
|
||||||
|
public void Start()
|
||||||
|
{
|
||||||
|
stopwatch.Start();
|
||||||
|
stopwatch_last = stopwatch.Elapsed.TotalMilliseconds;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void Step()
|
||||||
|
{
|
||||||
|
float elapsedMilliseconds = (float)(stopwatch.Elapsed.TotalMilliseconds - stopwatch_last);
|
||||||
|
stopwatch_last = stopwatch.Elapsed.TotalMilliseconds;
|
||||||
|
|
||||||
|
world.StepSimulation(elapsedMilliseconds / 1000.0f, 10);
|
||||||
|
|
||||||
|
// Detectar colisiones manualmente para sensores
|
||||||
|
CheckCollisions();
|
||||||
|
|
||||||
|
foreach (var cuerpo in Cuerpos)
|
||||||
|
{
|
||||||
|
if (cuerpo is simBotella botella)
|
||||||
|
{
|
||||||
|
botella.ApplyConveyorSpeed(elapsedMilliseconds / 1000);
|
||||||
|
if (botella.isRestricted)
|
||||||
|
{
|
||||||
|
botella.CenterFixtureOnConveyor();
|
||||||
|
botella.Body.SetMassProps(100, Vector3.Zero);
|
||||||
|
}
|
||||||
|
else if (botella.isNoMoreRestricted)
|
||||||
|
{
|
||||||
|
botella.isNoMoreRestricted = false;
|
||||||
|
botella.Body.SetMassProps(botella.OriginalMass, Vector3.Zero);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else if (cuerpo is simBarrera barrera)
|
||||||
|
barrera.CheckIfNecksIsTouching();
|
||||||
|
}
|
||||||
|
|
||||||
|
foreach (var action in _deferredActions)
|
||||||
|
{
|
||||||
|
action();
|
||||||
|
}
|
||||||
|
|
||||||
|
_deferredActions.Clear();
|
||||||
|
}
|
||||||
|
|
||||||
|
private void CheckCollisions()
|
||||||
|
{
|
||||||
|
int numManifolds = world.Dispatcher.NumManifolds;
|
||||||
|
|
||||||
|
for (int i = 0; i < numManifolds; i++)
|
||||||
|
{
|
||||||
|
PersistentManifold contactManifold = world.Dispatcher.GetManifoldByIndexInternal(i);
|
||||||
|
var obA = contactManifold.Body0;
|
||||||
|
var obB = contactManifold.Body1;
|
||||||
|
|
||||||
|
var bodyA = RigidBody.Upcast(obA);
|
||||||
|
var bodyB = RigidBody.Upcast(obB);
|
||||||
|
|
||||||
|
if (bodyA?.UserObject is simBotella botella && bodyB?.UserObject is simBase other)
|
||||||
|
{
|
||||||
|
HandleCollision(botella, other, bodyB);
|
||||||
|
}
|
||||||
|
else if (bodyB?.UserObject is simBotella botella2 && bodyA?.UserObject is simBase other2)
|
||||||
|
{
|
||||||
|
HandleCollision(botella2, other2, bodyA);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
private void HandleCollision(simBotella botella, simBase other, RigidBody otherBody)
|
||||||
|
{
|
||||||
|
switch (other)
|
||||||
|
{
|
||||||
|
case simBarrera sensor:
|
||||||
|
if (!sensor.ListSimBotellaContact.Contains(botella))
|
||||||
|
{
|
||||||
|
sensor.LuzCortada += 1;
|
||||||
|
sensor.ListSimBotellaContact.Add(botella);
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case simCurve curve:
|
||||||
|
if (!botella.ListOnTransports.Contains(curve))
|
||||||
|
{
|
||||||
|
botella.isOnTransports += 1;
|
||||||
|
botella.ListOnTransports.Add(curve);
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case simDescarte:
|
||||||
|
botella.Descartar = true;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case simTransporte conveyor:
|
||||||
|
if (!botella.ListOnTransports.Contains(conveyor))
|
||||||
|
{
|
||||||
|
botella.isOnTransports += 1;
|
||||||
|
botella.ListOnTransports.Add(conveyor);
|
||||||
|
|
||||||
|
if (conveyor.TransportWithGuides && conveyor.isBrake)
|
||||||
|
{
|
||||||
|
botella.ConveyorRestrictedTo = conveyor;
|
||||||
|
botella.axisRestrictedBy = otherBody;
|
||||||
|
botella.OriginalMass = botella.Body.Mass;
|
||||||
|
botella.isRestricted = true;
|
||||||
|
botella.isNoMoreRestricted = false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
public void Remove(simBase Objeto)
|
||||||
|
{
|
||||||
|
if (Objeto != null)
|
||||||
|
{
|
||||||
|
Objeto.RemoverBody();
|
||||||
|
Cuerpos.Remove(Objeto);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
public simCurve AddCurve(float innerRadius, float outerRadius, float startAngle, float endAngle, Vector3 position)
|
||||||
|
{
|
||||||
|
simCurve curva = new simCurve(world, _deferredActions, innerRadius, outerRadius, startAngle, endAngle, position);
|
||||||
|
Cuerpos.Add(curva);
|
||||||
|
return curva;
|
||||||
|
}
|
||||||
|
|
||||||
|
public simBotella AddCircle(float diameter, Vector3 position, float mass)
|
||||||
|
{
|
||||||
|
simBotella circle = new simBotella(world, _deferredActions, diameter, position, mass);
|
||||||
|
Cuerpos.Add(circle);
|
||||||
|
return circle;
|
||||||
|
}
|
||||||
|
|
||||||
|
public simTransporte AddRectangle(float width, float height, Vector3 position, float angle)
|
||||||
|
{
|
||||||
|
simTransporte rectangle = new simTransporte(world, _deferredActions, width, height, position, angle);
|
||||||
|
Cuerpos.Add(rectangle);
|
||||||
|
return rectangle;
|
||||||
|
}
|
||||||
|
|
||||||
|
public simBarrera AddBarrera(float width, float height, Vector3 position, float angle, bool detectarCuello)
|
||||||
|
{
|
||||||
|
simBarrera rectangle = new simBarrera(world, _deferredActions, width, height, position, angle, detectarCuello);
|
||||||
|
Cuerpos.Add(rectangle);
|
||||||
|
return rectangle;
|
||||||
|
}
|
||||||
|
|
||||||
|
public simGuia AddLine(Vector3 start, Vector3 end)
|
||||||
|
{
|
||||||
|
simGuia line = new simGuia(world, _deferredActions, start, end);
|
||||||
|
Cuerpos.Add(line);
|
||||||
|
return line;
|
||||||
|
}
|
||||||
|
|
||||||
|
public simDescarte AddDescarte(float diameter, Vector3 position)
|
||||||
|
{
|
||||||
|
simDescarte descarte = new simDescarte(world, _deferredActions, diameter, position);
|
||||||
|
Cuerpos.Add(descarte);
|
||||||
|
return descarte;
|
||||||
|
}
|
||||||
|
|
||||||
|
~SimulationManagerFP()
|
||||||
|
{
|
||||||
|
Clear();
|
||||||
|
world?.Dispose();
|
||||||
|
solver?.Dispose();
|
||||||
|
broadphase?.Dispose();
|
||||||
|
dispatcher?.Dispose();
|
||||||
|
collisionConfiguration?.Dispose();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
|
@ -11,6 +11,6 @@
|
||||||
|
|
||||||
<Grid Width="50" Height="50">
|
<Grid Width="50" Height="50">
|
||||||
<Ellipse Fill="{Binding Color_oculto}" Stroke="DarkGray" StrokeThickness="2" Width="30" Height="30" />
|
<Ellipse Fill="{Binding Color_oculto}" Stroke="DarkGray" StrokeThickness="2" Width="30" Height="30" />
|
||||||
|
<Label Content="{Binding Valor_Actual}" VerticalAlignment="Center" HorizontalAlignment="Center" FontSize="8" />
|
||||||
</Grid>
|
</Grid>
|
||||||
</UserControl>
|
</UserControl>
|
||||||
|
|
|
@ -514,9 +514,6 @@ namespace CtrEditor.Simulacion
|
||||||
public float OverlapPercentage;
|
public float OverlapPercentage;
|
||||||
public float _neckRadius;
|
public float _neckRadius;
|
||||||
PrismaticJoint _activeJoint;
|
PrismaticJoint _activeJoint;
|
||||||
public bool isBeingPressedByOtherBottles;
|
|
||||||
public int bottleContactCount;
|
|
||||||
public float debugPenetrationLevel; // Para debug: nivel de penetración actual
|
|
||||||
private List<Action> _deferredActions;
|
private List<Action> _deferredActions;
|
||||||
public simBotella(World world, List<Action> deferredActions, float diameter, Vector2 position, float mass, float neckRadius = 0)
|
public simBotella(World world, List<Action> deferredActions, float diameter, Vector2 position, float mass, float neckRadius = 0)
|
||||||
{
|
{
|
||||||
|
@ -586,8 +583,6 @@ namespace CtrEditor.Simulacion
|
||||||
RemoverBody();
|
RemoverBody();
|
||||||
isOnTransports = 0;
|
isOnTransports = 0;
|
||||||
_activeJoint = null;
|
_activeJoint = null;
|
||||||
isBeingPressedByOtherBottles = false;
|
|
||||||
bottleContactCount = 0;
|
|
||||||
Body = _world.CreateCircle(Radius, 1f, position);
|
Body = _world.CreateCircle(Radius, 1f, position);
|
||||||
Body.BodyType = BodyType.Dynamic;
|
Body.BodyType = BodyType.Dynamic;
|
||||||
// Restablecer manejador de eventos de colisión
|
// Restablecer manejador de eventos de colisión
|
||||||
|
@ -597,8 +592,8 @@ namespace CtrEditor.Simulacion
|
||||||
// Configurar la fricción
|
// Configurar la fricción
|
||||||
//Body.SetFriction(0.2f);
|
//Body.SetFriction(0.2f);
|
||||||
// Configurar amortiguamiento
|
// Configurar amortiguamiento
|
||||||
Body.LinearDamping = 3f; // Ajustar para controlar la reducción de la velocidad lineal
|
Body.LinearDamping = 5f; // Ajustar para controlar la reducción de la velocidad lineal
|
||||||
Body.AngularDamping = 1f; // Ajustar para controlar la reducción de la velocidad angular
|
Body.AngularDamping = 21f; // Ajustar para controlar la reducción de la velocidad angular
|
||||||
//Body.SetRestitution(0f); // Baja restitución para menos rebote
|
//Body.SetRestitution(0f); // Baja restitución para menos rebote
|
||||||
Body.SleepingAllowed = false;
|
Body.SleepingAllowed = false;
|
||||||
Body.IsBullet = true;
|
Body.IsBullet = true;
|
||||||
|
@ -653,11 +648,6 @@ namespace CtrEditor.Simulacion
|
||||||
|
|
||||||
return true; // No aplicar respuestas físicas
|
return true; // No aplicar respuestas físicas
|
||||||
}
|
}
|
||||||
else if (fixtureB.Body.Tag is simBotella)
|
|
||||||
{
|
|
||||||
bottleContactCount++;
|
|
||||||
return true; // Permitir respuestas físicas entre botellas
|
|
||||||
}
|
|
||||||
|
|
||||||
return true; // No aplicar respuestas físicas
|
return true; // No aplicar respuestas físicas
|
||||||
}
|
}
|
||||||
|
@ -712,11 +702,6 @@ namespace CtrEditor.Simulacion
|
||||||
Sensor.LuzCortada -= 1;
|
Sensor.LuzCortada -= 1;
|
||||||
Sensor.ListSimBotellaContact.Remove(this);
|
Sensor.ListSimBotellaContact.Remove(this);
|
||||||
}
|
}
|
||||||
|
|
||||||
if (fixtureB.Body.Tag is simBotella)
|
|
||||||
{
|
|
||||||
bottleContactCount = Math.Max(0, bottleContactCount - 1);
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
|
@ -768,17 +753,8 @@ namespace CtrEditor.Simulacion
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Si la botella está siendo presionada por múltiples botellas, reducir la aplicación de fuerza
|
// Aplicar la fuerza de fricción en función del porcentaje de superficie sobrepuesta
|
||||||
float pressureReductionFactor = 1.0f;
|
Body.LinearVelocity += frictionCoefficient * desiredVelocity;
|
||||||
if (isBeingPressedByOtherBottles)
|
|
||||||
{
|
|
||||||
// Reducir el factor basándose en la cantidad de contactos
|
|
||||||
pressureReductionFactor = Math.Max(0.1f, 1.0f / (1.0f + bottleContactCount * 0.5f));
|
|
||||||
}
|
|
||||||
|
|
||||||
// Aplicar la fuerza de fricción en función del porcentaje de superficie sobrepuesta y la presión
|
|
||||||
Vector2 finalVelocityChange = frictionCoefficient * desiredVelocity * pressureReductionFactor;
|
|
||||||
Body.LinearVelocity += finalVelocityChange;
|
|
||||||
//Body.ApplyForce(frictionForce * overlapPercentage);
|
//Body.ApplyForce(frictionForce * overlapPercentage);
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
@ -866,9 +842,6 @@ namespace CtrEditor.Simulacion
|
||||||
_deferredActions = new List<Action>();
|
_deferredActions = new List<Action>();
|
||||||
stopwatch = new Stopwatch();
|
stopwatch = new Stopwatch();
|
||||||
stopwatch.Start();
|
stopwatch.Start();
|
||||||
|
|
||||||
// Configurar el callback PreSolve para manejar contactos entre botellas
|
|
||||||
world.ContactManager.PreSolve += OnPreSolve;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
public void Clear()
|
public void Clear()
|
||||||
|
@ -889,20 +862,13 @@ namespace CtrEditor.Simulacion
|
||||||
|
|
||||||
public void Step()
|
public void Step()
|
||||||
{
|
{
|
||||||
// Resetear las flags de presión al inicio del frame
|
|
||||||
ResetBottlePressureFlags();
|
|
||||||
|
|
||||||
// Verificar y corregir el espaciado entre botellas antes del step
|
|
||||||
PreventBottleOverlapping();
|
|
||||||
|
|
||||||
// Actualizar el estado de presión basado en contactos reales
|
|
||||||
UpdateBottlePressureStatus();
|
|
||||||
|
|
||||||
// Detener el cronómetro y obtener el tiempo transcurrido en milisegundos
|
// Detener el cronómetro y obtener el tiempo transcurrido en milisegundos
|
||||||
float elapsedMilliseconds = (float)(stopwatch.Elapsed.TotalMilliseconds - stopwatch_last);
|
float elapsedMilliseconds = (float)(stopwatch.Elapsed.TotalMilliseconds - stopwatch_last);
|
||||||
stopwatch_last = stopwatch.Elapsed.TotalMilliseconds;
|
stopwatch_last = stopwatch.Elapsed.TotalMilliseconds;
|
||||||
// Pasar el tiempo transcurrido al método Step
|
// Pasar el tiempo transcurrido al método Step
|
||||||
|
|
||||||
world.Step(elapsedMilliseconds / 1000.0f);
|
world.Step(elapsedMilliseconds / 1000.0f);
|
||||||
|
|
||||||
foreach (var cuerpo in Cuerpos)
|
foreach (var cuerpo in Cuerpos)
|
||||||
{
|
{
|
||||||
if (cuerpo is simBotella botella)
|
if (cuerpo is simBotella botella)
|
||||||
|
@ -912,7 +878,7 @@ namespace CtrEditor.Simulacion
|
||||||
{
|
{
|
||||||
botella.CenterFixtureOnConveyor();
|
botella.CenterFixtureOnConveyor();
|
||||||
botella.Body.Inertia = 0;
|
botella.Body.Inertia = 0;
|
||||||
botella.Body.Mass = botella.OriginalMass * 100;
|
botella.Body.Mass = 100;
|
||||||
}
|
}
|
||||||
else if (botella.isNoMoreRestricted)
|
else if (botella.isNoMoreRestricted)
|
||||||
{
|
{
|
||||||
|
@ -1084,212 +1050,5 @@ namespace CtrEditor.Simulacion
|
||||||
|
|
||||||
return polygon;
|
return polygon;
|
||||||
}
|
}
|
||||||
|
|
||||||
private void OnPreSolve(Contact contact, ref nkast.Aether.Physics2D.Collision.Manifold oldManifold)
|
|
||||||
{
|
|
||||||
var fixtureA = contact.FixtureA;
|
|
||||||
var fixtureB = contact.FixtureB;
|
|
||||||
|
|
||||||
// Verificar si el contacto es entre dos botellas
|
|
||||||
if (fixtureA.Body.Tag is simBotella botellaA && fixtureB.Body.Tag is simBotella botellaB)
|
|
||||||
{
|
|
||||||
// Calcular la distancia entre centros de las botellas
|
|
||||||
Vector2 centerA = botellaA.Body.Position;
|
|
||||||
Vector2 centerB = botellaB.Body.Position;
|
|
||||||
Vector2 direction = centerB - centerA;
|
|
||||||
float distance = direction.Length();
|
|
||||||
|
|
||||||
// Distancia mínima permitida (suma de radios + margen de seguridad)
|
|
||||||
float minDistance = botellaA.Radius + botellaB.Radius + 0.01f; // 1cm de margen
|
|
||||||
|
|
||||||
// Calcular la velocidad relativa
|
|
||||||
Vector2 relativeVelocity = botellaB.Body.LinearVelocity - botellaA.Body.LinearVelocity;
|
|
||||||
float relativeSpeed = relativeVelocity.Length();
|
|
||||||
|
|
||||||
// Solo marcar como presionadas si hay verdadera interpenetración O múltiples contactos
|
|
||||||
if (distance < minDistance * 0.95f) // Solo si están muy cerca (interpenetración significativa)
|
|
||||||
{
|
|
||||||
botellaA.isBeingPressedByOtherBottles = true;
|
|
||||||
botellaB.isBeingPressedByOtherBottles = true;
|
|
||||||
|
|
||||||
// Ajustar propiedades del contacto para reducir fuerzas explosivas
|
|
||||||
float penetrationRatio = Math.Max(0, (minDistance - distance) / minDistance);
|
|
||||||
|
|
||||||
// Reducir restitución progresivamente según la penetración
|
|
||||||
contact.Restitution = Math.Min(contact.Restitution, 0.1f * (1.0f - penetrationRatio));
|
|
||||||
|
|
||||||
// Aumentar fricción para disipar energía
|
|
||||||
contact.Friction = Math.Max(contact.Friction, 0.7f + penetrationRatio * 0.2f);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Si la velocidad relativa es muy alta, también ajustar contacto
|
|
||||||
if (relativeSpeed > 2.0f)
|
|
||||||
{
|
|
||||||
contact.Restitution = 0.0f; // Sin rebote
|
|
||||||
contact.Friction = Math.Max(contact.Friction, 0.9f);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
public void ResetBottlePressureFlags()
|
|
||||||
{
|
|
||||||
foreach (var cuerpo in Cuerpos)
|
|
||||||
{
|
|
||||||
if (cuerpo is simBotella botella)
|
|
||||||
{
|
|
||||||
botella.isBeingPressedByOtherBottles = false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
private void UpdateBottlePressureStatus()
|
|
||||||
{
|
|
||||||
var botellas = new List<simBotella>();
|
|
||||||
|
|
||||||
// Recopilar todas las botellas
|
|
||||||
foreach (var cuerpo in Cuerpos)
|
|
||||||
{
|
|
||||||
if (cuerpo is simBotella botella)
|
|
||||||
{
|
|
||||||
botellas.Add(botella);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Verificar el estado de presión de cada botella basado en sus contactos reales
|
|
||||||
foreach (var botella in botellas)
|
|
||||||
{
|
|
||||||
int proximityCount = 0;
|
|
||||||
float totalPenetration = 0;
|
|
||||||
|
|
||||||
foreach (var otherBottle in botellas)
|
|
||||||
{
|
|
||||||
if (botella == otherBottle) continue;
|
|
||||||
|
|
||||||
Vector2 direction = otherBottle.Body.Position - botella.Body.Position;
|
|
||||||
float distance = direction.Length();
|
|
||||||
float minDistance = botella.Radius + otherBottle.Radius + 0.01f; // 1cm margen
|
|
||||||
|
|
||||||
if (distance < minDistance)
|
|
||||||
{
|
|
||||||
proximityCount++;
|
|
||||||
totalPenetration += (minDistance - distance);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Actualizar información de debug
|
|
||||||
botella.debugPenetrationLevel = totalPenetration;
|
|
||||||
|
|
||||||
// Solo marcar como presionada si tiene múltiples contactos cercanos O penetración significativa
|
|
||||||
// Y además la botella está en un transportador (para evitar marcar botellas libres)
|
|
||||||
if ((proximityCount >= 2 || totalPenetration > 0.01f) && botella.IsOnAnyTransport())
|
|
||||||
{
|
|
||||||
botella.isBeingPressedByOtherBottles = true;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
private void PreventBottleOverlapping()
|
|
||||||
{
|
|
||||||
var botellas = new List<simBotella>();
|
|
||||||
|
|
||||||
// Recopilar todas las botellas
|
|
||||||
foreach (var cuerpo in Cuerpos)
|
|
||||||
{
|
|
||||||
if (cuerpo is simBotella botella)
|
|
||||||
{
|
|
||||||
botellas.Add(botella);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Verificar cada par de botellas múltiples veces para resolver interpenetraciones complejas
|
|
||||||
int iterations = 3; // Múltiples iteraciones para resolver conflictos
|
|
||||||
|
|
||||||
for (int iter = 0; iter < iterations; iter++)
|
|
||||||
{
|
|
||||||
bool anyCorrection = false;
|
|
||||||
|
|
||||||
for (int i = 0; i < botellas.Count; i++)
|
|
||||||
{
|
|
||||||
for (int j = i + 1; j < botellas.Count; j++)
|
|
||||||
{
|
|
||||||
var botellaA = botellas[i];
|
|
||||||
var botellaB = botellas[j];
|
|
||||||
|
|
||||||
Vector2 centerA = botellaA.Body.Position;
|
|
||||||
Vector2 centerB = botellaB.Body.Position;
|
|
||||||
Vector2 direction = centerB - centerA;
|
|
||||||
float distance = direction.Length();
|
|
||||||
|
|
||||||
// Distancia mínima permitida (suma de radios + margen de seguridad)
|
|
||||||
float minDistance = botellaA.Radius + botellaB.Radius + 0.005f; // 5mm de margen
|
|
||||||
|
|
||||||
// Si están demasiado cerca
|
|
||||||
if (distance < minDistance && distance > 0.001f)
|
|
||||||
{
|
|
||||||
anyCorrection = true;
|
|
||||||
|
|
||||||
// Normalizar la dirección
|
|
||||||
direction.Normalize();
|
|
||||||
|
|
||||||
// Calcular la corrección necesaria
|
|
||||||
float penetration = minDistance - distance;
|
|
||||||
float correction = penetration * 0.5f;
|
|
||||||
|
|
||||||
// Factor de corrección que decrece con las iteraciones
|
|
||||||
float correctionFactor = 0.3f / (iter + 1); // Más agresivo en la primera iteración
|
|
||||||
Vector2 correctionVector = direction * correction * correctionFactor;
|
|
||||||
|
|
||||||
// Aplicar corrección considerando masas (botellas más pesadas se mueven menos)
|
|
||||||
float massA = botellaA.Body.Mass;
|
|
||||||
float massB = botellaB.Body.Mass;
|
|
||||||
float totalMass = massA + massB;
|
|
||||||
|
|
||||||
float ratioA = massB / totalMass; // Botella más liviana se mueve más
|
|
||||||
float ratioB = massA / totalMass;
|
|
||||||
|
|
||||||
// Solo mover si las botellas no están restringidas por transportadores
|
|
||||||
if (!botellaA.isRestricted)
|
|
||||||
{
|
|
||||||
Vector2 newPosA = centerA - correctionVector * ratioA;
|
|
||||||
if (!float.IsNaN(newPosA.X) && !float.IsNaN(newPosA.Y))
|
|
||||||
{
|
|
||||||
botellaA.Body.Position = newPosA;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
if (!botellaB.isRestricted)
|
|
||||||
{
|
|
||||||
Vector2 newPosB = centerB + correctionVector * ratioB;
|
|
||||||
if (!float.IsNaN(newPosB.X) && !float.IsNaN(newPosB.Y))
|
|
||||||
{
|
|
||||||
botellaB.Body.Position = newPosB;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Solo marcar como presionadas si hay interpenetración significativa
|
|
||||||
if (penetration > 0.005f) // Solo si penetran más de 5mm
|
|
||||||
{
|
|
||||||
botellaA.isBeingPressedByOtherBottles = true;
|
|
||||||
botellaB.isBeingPressedByOtherBottles = true;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Reducir velocidades para evitar acumulación de energía
|
|
||||||
float velocityDamping = 0.9f;
|
|
||||||
if (penetration > 0.01f) // Si la penetración es significativa (> 1cm)
|
|
||||||
{
|
|
||||||
velocityDamping = 0.7f; // Reducción más agresiva
|
|
||||||
}
|
|
||||||
|
|
||||||
botellaA.Body.LinearVelocity *= velocityDamping;
|
|
||||||
botellaB.Body.LinearVelocity *= velocityDamping;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Si no hubo correcciones en esta iteración, podemos parar
|
|
||||||
if (!anyCorrection)
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
}
|
}
|
Loading…
Reference in New Issue