Source code for pyunity.physics.core

## Copyright (c) 2020-2022 The PyUnity Team
## This file is licensed under the MIT License.
## See https://docs.pyunity.x10.bz/en/latest/license.html

"""
Core classes of the PyUnity
physics engine.

"""

__all__ = ["PhysicMaterial", "Collider", "SphereCollider", "Manifold",
           "BoxCollider", "Rigidbody", "Infinity"]

from ..errors import PyUnityException
from ..values import Vector3, Quaternion, ABCMeta, abstractmethod, IgnoredMixin
from ..core import Component, ShowInInspector, addFields
from . import config
import math

Infinity = math.inf
"""A representation of infinity"""

[docs]class PhysicMaterial: """ Class to store data on a collider's material. Parameters ---------- restitution : float Bounciness of the material friction : float Friction of the material Attributes ---------- restitution : float Bounciness of the material friction : float Friction of the material combine : int Combining function. -1 means minimum, 0 means average, and 1 means maximum """
[docs] def exception(self, *args, **kwargs): raise PyUnityException( "Cannot modify properties of PhysicMaterial: it is immutable")
def __init__(self, restitution=0.75, friction=1, immutable=False): self.restitution = restitution self.friction = friction self.combine = -1 if immutable: self.__setattr__ = self.exception
[docs]class Manifold: """ Class to store collision data. Parameters ---------- a : Collider The first collider b : Collider The second collider point : Vector3 The collision point normal : Vector3 The collision normal penetration : float How much the two colliders overlap """ def __init__(self, a, b, point, normal, penetration): self.a = a self.b = b self.point = point self.normal = normal self.penetration = penetration def __str__(self): return f"<Manifold point={self.point} normal={self.normal} penetration={self.penetration}>"
[docs]class Collider(Component, metaclass=ABCMeta): """ Collider base class. Attributes ---------- offset : Vector3 The offset from the centre of the Collider """ offset = ShowInInspector(Vector3)
[docs] @abstractmethod def supportPoint(self, direction): pass
@property def pos(self): return self.transform.position @pos.setter def pos(self, value): self.transform.position = value @property def rot(self): return self.transform.rotation @rot.setter def rot(self, value): self.transform.rotation = value
[docs]class SphereCollider(Collider): """ A spherical collider that cannot be deformed. Attributes ---------- radius : Vector3 The radius of the SphereCollider """ radius = ShowInInspector(float, 0) def __init__(self, transform): super(SphereCollider, self).__init__(transform) self.SetSize(max(self.transform.scale.abs()), Vector3.zero())
[docs] def SetSize(self, radius, offset): """ Sets the size of the collider. Parameters ---------- radius : float The radius of the collider. offset : Vector3 Offset of the collider. """ self.radius = radius self.offset = offset
@property def min(self): return self.pos - self.radius @property def max(self): return self.pos + self.radius
[docs] def collidingWith(self, other): if isinstance(other, SphereCollider): radii = (self.radius + other.radius) ** 2 distance = self.pos.getDistSqrd(other.pos) if distance < radii: relative = (other.pos - self.pos).normalized() dist = self.radius + other.radius - math.sqrt(distance) return Manifold(self, other, (self.pos + other.pos) / 2, relative, dist) else: return CollManager.epa(self, other)
[docs] def supportPoint(self, direction): return self.pos + direction.normalized() * self.radius
[docs]class BoxCollider(Collider): """ An axis-aligned box collider that cannot be deformed. Attributes ---------- size : Vector3 The distance between two farthest vertices of the collider """ size = ShowInInspector(Vector3) def __init__(self, transform): super(BoxCollider, self).__init__(transform) self.SetSize(self.transform.scale * 2, Vector3.zero())
[docs] def SetSize(self, size, offset): """ Sets the size of the collider. Parameters ---------- size : Vector3 The dimensions of the collider. offset : Vector3 Offset of the collider. """ self.size = size self.offset = offset
@property def min(self): return self.pos - self.rot.RotateVector(self.size / 2) @property def max(self): return self.pos + self.rot.RotateVector(self.size / 2)
[docs] def collidingWith(self, other): return CollManager.epa(self, other)
[docs] def supportPoint(self, direction): def sign(a): return -1 if a < 0 else 1 newdir = self.transform.rotation.conjugate.RotateVector(direction) point = newdir._o1(sign) * self.size / 2 res = self.transform.rotation.RotateVector(point) return res + self.transform.position
[docs]@addFields( mass=ShowInInspector(float, 100), inertia=ShowInInspector(float, 200 / 3)) class Rigidbody(Component): """ Class to let a GameObject follow physics rules. Attributes ---------- velocity : Vector3 Velocity of the Rigidbody rotVel : Vector3 Rotational velocity of the Rigidbody force : Vector3 Force acting on the Rigidbody. Reset every frame. torque : Vector3 Rotational force acting on the Rigidbody. Reset every frame. physicMaterial : PhysicMaterial Physics material of the Rigidbody """ velocity = ShowInInspector(Vector3) rotVel = ShowInInspector(Vector3, None, "Rotational Velocity") force = ShowInInspector(Vector3) torque = ShowInInspector(Vector3) gravity = ShowInInspector(bool, True) physicMaterial = ShowInInspector( PhysicMaterial, PhysicMaterial(immutable=True)) def __init__(self, transform, dummy=False): super(Rigidbody, self).__init__(transform, dummy) self.mass = 100 self.velocity = Vector3.zero() self.rotVel = Vector3.zero() self.force = Vector3.zero() self.torque = Vector3.zero() @property def mass(self): """Mass of the Rigidbody. Defaults to 100""" if self.invMass == 0: return Infinity return 1 / self.invMass @mass.setter def mass(self, val): if val == Infinity or val == 0: self.invMass = 0 else: self.invMass = 1 / val self.inertia = 2 / 3 * self.mass # (1/6 ms^2) @property def inertia(self): if self.invInertia == 0: return Infinity return 1 / self.invInertia @inertia.setter def inertia(self, val): if val == Infinity or val == 0: self.invInertia = 0 self.invInertia = 1 / val @property def pos(self): return self.transform.position @pos.setter def pos(self, val): self.transform.position = val @property def rot(self): return self.transform.rotation @rot.setter def rot(self, val): self.transform.rotation = val
[docs] def Move(self, dt): """ Moves all colliders on the GameObject by the Rigidbody's velocity times the delta time. Parameters ---------- dt : float Time to simulate movement by """ if self.gravity and self.invMass > 0: self.force += config.gravity * self.mass self.velocity += self.force * self.invMass * dt self.pos += self.velocity * dt self.rotVel += self.torque * self.invInertia rotation = self.rotVel * dt angle = rotation.length if angle != 0: rotation /= angle rotQuat = Quaternion.FromAxis(math.degrees(angle), rotation) self.rot *= rotQuat self.force = Vector3.zero() self.torque = Vector3.zero()
[docs] def MovePos(self, offset): """ Moves the rigidbody and its colliders by an offset. Parameters ---------- offset : Vector3 Offset to move """ self.pos += offset
[docs] def AddForce(self, force, point=Vector3.zero()): """ Apply a force to the center of the Rigidbody. Parameters ---------- force : Vector3 Force to apply point : Vector3, optional Point relative to center of mass in local space to apply force at Notes ----- A force is a gradual change in velocity, whereas an impulse is just a jump in velocity. """ self.force += force self.torque += point.cross(force)
[docs] def AddImpulse(self, impulse): """ Apply an impulse to the center of the Rigidbody. Parameters ---------- impulse : Vector3 Impulse to apply Notes ----- A force is a gradual change in velocity, whereas an impulse is just a jump in velocity. """ self.velocity += impulse
[docs]class SupportPoint(IgnoredMixin): def __init__(self, point, original): self.point = point self.original = original
[docs]class Triangle(IgnoredMixin): def __init__(self, a, b, c): self.a = a self.b = b self.c = c ab = b.point - a.point ac = c.point - a.point self.normal = (ab).cross(ac).normalized()
[docs]class CollManager(IgnoredMixin): """ Manages the collisions between all colliders. Attributes ---------- rigidbodies : dict Dictionary of rigidbodies andthe colliders on the gameObject that the Rigidbody belongs to dummyRigidbody : Rigidbody A dummy rigidbody used when a GameObject has colliders but no rigidbody. It has infinite mass """ def __init__(self): self.rigidbodies = {} self.dummyRigidbody = Rigidbody(None, True) self.dummyRigidbody.mass = Infinity self.steps = 1
[docs] @staticmethod def supportPoint(a, b, direction): supportA = a.supportPoint(direction) supportB = b.supportPoint(-direction) support = supportA - supportB return SupportPoint(support, (supportA, supportB))
[docs] @staticmethod def nextSimplex(args): length = len(args[0]) if length == 2: return CollManager.lineSimplex(args) if length == 3: return CollManager.triSimplex(args) if length == 4: return CollManager.tetraSimplex(args) return False
[docs] @staticmethod def lineSimplex(args): a, b = [x.point for x in args[0]] ab = b - a ao = -a if ab.dot(ao) > 0: args[1] = ao - ab / 2 else: args[0] = [args[0][0]] args[1] = ao return False
[docs] @staticmethod def triSimplex(args): a, b, c = [x.point for x in args[0]] ab = b - a ac = c - a ao = -a abc = ab.cross(ac) if abc.cross(ac).dot(ao) > 0: if ac.dot(ao) > 0: args[0] = [args[0][0], args[0][2]] args[1] = ao - ac / 2 else: args[0] = [args[0][0], args[0][1]] return CollManager.lineSimplex(args) elif ab.cross(abc).dot(ao) > 0: args[0] = [args[0][0], args[0][1]] return CollManager.lineSimplex(args) else: if abc.dot(ao) > 0: args[1] = abc else: args[0] = [args[0][0], args[0][2], args[0][1]] args[1] = -abc return False
[docs] @staticmethod def tetraSimplex(args): a, b, c, d = [x.point for x in args[0]] ab = b - a ac = c - a ad = d - a ao = -a abc = ab.cross(ac) acd = ac.cross(ad) adb = ad.cross(ab) if abc.dot(ao) > 0: args[0] = [args[0][0], args[0][1], args[0][2]] return CollManager.triSimplex(args) if acd.dot(ao) > 0: args[0] = [args[0][0], args[0][2], args[0][3]] return CollManager.triSimplex(args) if adb.dot(ao) > 0: args[0] = [args[0][0], args[0][3], args[0][1]] return CollManager.triSimplex(args) return True
[docs] @staticmethod def gjk(a, b): ab = a.pos - b.pos c = Vector3(ab.z, ab.z, -ab.x - ab.y) if c == Vector3.zero(): c = Vector3(-ab.y - ab.z, ab.x, ab.x) support = CollManager.supportPoint(a, b, ab.cross(c)) points = [support] direction = -support.point maxIter = 50 i = 0 while True: if i >= maxIter: return None i += 1 support = CollManager.supportPoint(a, b, direction) if support.point.dot(direction) <= 0: return None points.insert(0, support) args = [points, direction] if CollManager.nextSimplex(args): return args[0] points, direction = args
[docs] @staticmethod def epa(a, b): # https://blog.winter.dev/2020/epa-algorithm/ points = CollManager.gjk(a, b) if points is None: return [] p0, p1, p2, p3 = points triangles = [] edges = [] threshold = 1e-8 limit = 50 cur = 0 triangles.append(Triangle(p0, p1, p2)) triangles.append(Triangle(p0, p2, p3)) triangles.append(Triangle(p0, p3, p1)) triangles.append(Triangle(p1, p3, p2)) while True: if cur >= limit: return [] cur += 1 results = [(t, abs(t.normal.dot(t.a.point))) for t in triangles] results.sort(key=lambda x: x[1]) results = [r for r in results if abs(r[1] - results[0][1]) < 0.001] curTriangle = None for result, dst in results: minSupport = CollManager.supportPoint(a, b, result.normal) if result.normal.dot(minSupport.point) - dst < threshold: curTriangle = result if curTriangle is not None: break i = 0 while i < len(triangles): triangle = triangles[i] if triangle.normal.dot(minSupport.point - triangle.a.point) > 0: CollManager.AddEdge(edges, triangle.a, triangle.b) CollManager.AddEdge(edges, triangle.b, triangle.c) CollManager.AddEdge(edges, triangle.c, triangle.a) triangles.remove(triangle) continue i += 1 for edge in edges: triangles.append(Triangle(minSupport, edge[0], edge[1])) edges.clear() penetration = curTriangle.normal.dot(curTriangle.a.point) u, v, w = CollManager.barycentric( curTriangle.normal * penetration, curTriangle.a.point, curTriangle.b.point, curTriangle.c.point) if abs(u) > 1 or abs(v) > 1 or abs(w) > 1: return None elif math.isnan(u + v + w): return None point = Vector3( u * curTriangle.a.original[0] + v * curTriangle.b.original[0] + w * curTriangle.c.original[0]) normal = -curTriangle.normal return Manifold(a, b, point, normal, penetration)
[docs] @staticmethod def AddEdge(edges, a, b): if (b, a) in edges: edges.remove((b, a)) else: edges.append((a, b))
[docs] @staticmethod def barycentric(p, a, b, c): v0 = b - a v1 = c - a v2 = p - a d00 = v0.dot(v0) d01 = v0.dot(v1) d11 = v1.dot(v1) d20 = v2.dot(v0) d21 = v2.dot(v1) denom = d00 * d11 - d01 * d01 if denom == 0: print(p, a, b, c) v = (d11 * d20 - d01 * d21) / denom w = (d00 * d21 - d01 * d20) / denom u = 1 - v - w return u, v, w
[docs] def AddPhysicsInfo(self, scene): """ Get all colliders and rigidbodies from a specified scene. This overwrites the collider and rigidbody lists, and so can be called whenever a new collider or rigidbody is added or removed. Parameters ---------- scene : Scene Scene to search for physics info Notes ----- This function will overwrite the pre-existing dictionary of rigidbodies. When there are colliders but no rigidbody is on the GameObject, then they are placed in the dictionary with a dummy Rigidbody that has infinite mass and a default physic material. Thus, they cannot move. """ self.rigidbodies = {} dummies = [] for gameObject in scene.gameObjects: colliders = gameObject.GetComponents(Collider) rb = gameObject.GetComponent(Rigidbody) if rb is None: dummies.extend(colliders) continue self.rigidbodies[rb] = colliders self.rigidbodies[self.dummyRigidbody] = dummies
[docs] def GetRestitution(self, a, b): """ Get the restitution needed for two rigidbodies, based on their combine function Parameters ---------- a : Rigidbody Rigidbody 1 b : Rigidbody Rigidbody 2 Returns ------- float Restitution """ if a.physicMaterial.combine + b.physicMaterial.combine < 0: return min(a.physicMaterial.restitution, b.physicMaterial.restitution) elif a.physicMaterial.combine + b.physicMaterial.combine > 0: return max(a.physicMaterial.restitution, b.physicMaterial.restitution) else: return (a.physicMaterial.restitution + b.physicMaterial.restitution) / 2
[docs] def CheckCollisions(self): """ Goes through every pair exactly once, then checks their collisions and resolves them. """ manifolds = {} for x, rbA in zip(range(0, len(self.rigidbodies) - 1), list(self.rigidbodies.keys())[:-1]): for rbB in list(self.rigidbodies.keys())[x + 1:]: for colliderA in self.rigidbodies[rbA]: for colliderB in self.rigidbodies[rbB]: m = colliderA.collidingWith(colliderB) if m is not None: manifolds[rbA, rbB] = m for rbA, rbB in manifolds: m = manifolds[rbA, rbB] e = self.GetRestitution(rbA, rbB) self.ResolveCollisions( rbA, rbB, m.point, e, m.normal, m.penetration)
[docs] def ResolveCollisions(self, a, b, point, restitution, normal, penetration): # rv = b.velocity - a.velocity # vn = rv.dot(normal) # if vn < 0: # return if b is self.dummyRigidbody: ap = point - a.pos vab = a.velocity + a.rotVel.cross(ap) top = -(1 + restitution) * vab.dot(normal) apCrossN = ap.cross(normal) inertiaAcoeff = apCrossN.dot(apCrossN) * a.invInertia bottom = a.invMass + inertiaAcoeff j = top / bottom a.velocity += j * normal * a.invMass a.rotVel += (point.cross(j * normal)) * a.invInertia return ap = point - a.pos bp = point - b.pos vab = a.velocity + a.rotVel.cross(ap) - b.velocity - b.rotVel.cross(bp) apCrossN = ap.cross(normal) bpCrossN = bp.cross(normal) inertiaAcoeff = apCrossN.dot(apCrossN) * a.invInertia inertiaBcoeff = bpCrossN.dot(bpCrossN) * b.invInertia top = -(1 + restitution) * vab.dot(normal) bottom = a.invMass + b.invMass + inertiaAcoeff + inertiaBcoeff j = top / bottom a.velocity += j * normal * a.invMass b.velocity -= j * normal * b.invMass a.rotVel += (ap.cross(j * normal)) * a.invInertia b.rotVel -= (bp.cross(j * normal)) * b.invInertia # Positional correction percent = 0.6 slop = 0.01 correction = max(penetration - slop, 0) / (a.invMass + b.invMass) * percent * normal a.pos += a.invMass * correction b.pos -= b.invMass * correction
[docs] def correctInf(self, a, b, correction, target): if not math.isinf(a + b): return 1 / target * correction else: return 0
[docs] def Step(self, dt): """ Steps through the simulation at a given delta time. Parameters ---------- dt : float Delta time to step Notes ----- The simulation is stepped 10 times manually by the scene, so it is more precise. """ for rb in self.rigidbodies: if rb is not self.dummyRigidbody: rb.Move(dt) self.CheckCollisions()