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