public void Trace(Ray ray)
{
foreach (Model model in Models)
{
CollisionHelpers.RayIntersectsModel(ray, model, out bool insideBoundingSphere, out Vector3 vertex1, out Vector3 vertex2, out Vector3 vertex3, out ModelMeshPart intersectsWithPart, out ModelMesh intersectsWith);
if (intersectsWithPart != null)
{
Console.WriteLine(intersectsWith.Name);
Entities.Add(new Face {
Points = new List <Vector3> {
vertex1, vertex2, vertex3
}
});
}
}
}
void _advancePhysics(ref float dt)
{
List <CollisionInfo> contacts = new List <CollisionInfo>();
Vector3 pos = transform.position;
Quaternion rot = transform.rotation;
Vector3 velocity = Velocity;
Vector3 omega = AngularVelocity;
if (useUnityContacts)
{
foreach (var collision in _unityCollisions)
{
foreach (var contact in collision.contacts)
{
var penetration = -contact.separation;
var col = new CollisionInfo {
Penetration = penetration,
Restitution = 1,
Friction = 1,
Normal = contact.normal,
Point = contact.point
};
if (contact.otherCollider.attachedRigidbody != null)
{
col.Collider = contact.otherCollider;
col.Velocity = contact.otherCollider.attachedRigidbody.GetPointVelocity(contact.point);
}
contacts.Add(col);
Debug.DrawRay(contact.point, contact.normal, Color.blue, 5);
}
}
}
else
{
var radius = Radius + 0.0001f;
for (var index = 0; index < _colTests.Count; index++)
{
var meshCollider = _colTests[index];
var localPos = meshCollider.transform.InverseTransformPoint(pos);
var length = _meshes[index].Triangles.Length;
for (int i = 0; i < length; i += 3)
{
var triangle0 = _meshes[index].Triangles[i];
var triangle1 = _meshes[index].Triangles[i + 1];
var triangle2 = _meshes[index].Triangles[i + 2];
var p0 = _meshes[index].Vertices[triangle0];
var p1 = _meshes[index].Vertices[triangle1];
var p2 = _meshes[index].Vertices[triangle2];
var normal = Vector3.Cross(p2 - p0, p2 - p1).normalized;
if (CollisionHelpers.ClosestPtPointTriangle(localPos, radius, p0, p1, p2, normal, out var closest) == false)
{
continue;
}
Vector3 vector32 = localPos - closest;
if (vector32.sqrMagnitude > radius * radius)
{
continue;
}
Vector3 vector33 = localPos - closest;
if (Vector3.Dot(localPos - closest, normal) < 0.0)
{
continue;
}
vector33.Normalize();
CollisionInfo collisionInfo = new CollisionInfo()
{
Normal = meshCollider.transform.TransformDirection(vector33),
Point = meshCollider.transform.TransformPoint(closest)
};
Debug.DrawRay(collisionInfo.Point, collisionInfo.Normal, Color.red);
collisionInfo.Penetration = radius - Vector3.Dot(localPos - closest, collisionInfo.Normal);
collisionInfo.Restitution = 1f;
collisionInfo.Friction = 1f;
if (_colTests[index].attachedRigidbody != null)
{
collisionInfo.Velocity = _colTests[index].attachedRigidbody.GetPointVelocity(collisionInfo.Point);
}
contacts.Add(collisionInfo);
}
}
}
_updateMove(ref dt, ref velocity, ref omega, contacts);
// velocity += _gravityDir * _gravity * dt;
_updateIntegration(dt, ref pos, ref rot, velocity, omega);
if (useUnityContacts)
{
_rigidBody.MovePosition(pos);
_rigidBody.MoveRotation(rot);
}
else
{
transform.position = pos;
transform.rotation = rot;
}
Velocity = velocity;
AngularVelocity = omega;
}
