def find_point(self, node):
# Kod pisan po uzoru na http://www.dyn4j.org/2011/11/contact-points-using-clipping/
object1 = node.items[1] # incident object
object2 = node.items[0] # reference object
normal = node.value1
max_extreme_e1, point1_e1, point2_e1 = object1.getCoallisionEdge(-normal) # incident edge
max_extreme_e2, point1_e2, point2_e2 = object2.getCoallisionEdge(normal) # reference edge
#print(max_extreme_e1, point1_e1, point2_e1, object2.angle, normal)
if isinstance(object1, Ball):
return max_extreme_e1
if isinstance(object2, Ball):
d1 = object2.getCenter() - point1_e1
d2 = object2.getCenter() - point2_e1
d1 = np.linalg.norm(d1)
d2 = np.linalg.norm(d2)
if d1 < d2:
return point1_e1
return point2_e1
e2 = point2_e2 - point1_e2
e2 = e2 / np.linalg.norm(e2)
o1 = e2.dot(point1_e2)
clips = self.clip(point1_e1, point2_e1, e2, o1)
if len(clips) < 2:
return
o2 = e2.dot(point2_e2)
clips = self.clip(clips[0], clips[1], -e2, -o2)
if len(clips) < 2:
return
max_value = normal.dot(max_extreme_e2)
if normal.dot(clips[0]) - max_value > 0:
clips.pop(0)
if normal.dot(clips[0]) - max_value > 0:
clips.pop(0)
elif normal.dot(clips[1]) - max_value > 0:
clips.pop(1)
for c in clips:
Debug.draw_point(c, (50, 50, 50))
dot = None
if len(clips) == 2:
clips[0] = (clips[0] + clips[1])/2
clips.pop()
if len(clips) == 1:
dot = clips[0]
return dot
def collide(self):
# Kod pisan po uzoru na https://github.com/erincatto/box2d-lite/
coallisions = self.check_coallision()
if not coallisions:
return
self.cashed_data_new = {}
for i in range(len(coallisions)):
node = coallisions[i]
referenced_object = node.items[0]
incident_object = node.items[1]
referenced_object.updateSpeeds()
incident_object.updateSpeeds()
n = incident_object.getCenter() - referenced_object.getCenter()
if n.dot(node.value1) < 0:
node.value1 *= -1
normal = node.value1
point_of_coallision = self.find_point(node)
if point_of_coallision is None:
coallisions[i] = None
continue
# DRAW COLLISION POINTS
else:
Debug.draw_point(point_of_coallision)
r1 = point_of_coallision - referenced_object.getCenter()
r2 = point_of_coallision - incident_object.getCenter()
rn1 = r1.dot(normal)
rn2 = r2.dot(normal)
k_normal = 1/referenced_object.mass + 1/incident_object.mass
k_normal += referenced_object.inverse_inertia * (r1.dot(r1) - rn1*rn1) + incident_object.inverse_inertia * (r2.dot(r2) - rn2*rn2)
mass_normal = 1/k_normal
bias = -Fluid.BETA * (1/Shape.DELTA_TIME)*min(0, node.value2 + Fluid.SLOP)
tangent = cross(normal, 1)
rt1 = r1.dot(tangent)
rt2 = r2.dot(tangent)
k_tangent = 1/referenced_object.mass + 1/incident_object.mass
k_tangent += referenced_object.inverse_inertia * (r1.dot(r1) - rt1*rt1) + incident_object.inverse_inertia * (r2.dot(r2) - rt2*rt2)
mass_tangent = 1/k_tangent
node.mass_tangent = mass_tangent
node.r1 = r1
node.r2 = r2
node.mass_normal = mass_normal
node.bias = bias
friction = np.sqrt(referenced_object.friction * incident_object.friction)
node.friction = friction
key = (str(id(referenced_object)) + str(id(incident_object)))
if key in self.cashed_data_old:
node.normal_impulse = self.cashed_data_old[key].normal_impulse
node.tangent_impulse = self.cashed_data_old[key].tangent_impulse
self.cashed_data_new[key] = node
old_impuse = node.normal_impulse * normal + node.tangent_impulse*tangent
referenced_object.add_impulse(-old_impuse, node.r1)
incident_object.add_impulse(old_impuse, node.r2)
node.key = key
else:
node.normal_impulse = 0
node.tangent_impulse = 0
node.key = key
self.cashed_data_new[key] = node
for i in range(10):
for node in coallisions:
if node is None:
continue
referenced_object = node.items[0]
incident_object = node.items[1]
normal = node.value1
velocity = incident_object.vector + cross(incident_object.angularspeed, node.r2) - referenced_object.vector - cross(referenced_object.angularspeed, node.r1)
velocity_value = velocity.dot(normal)
normal_impulse = node.mass_normal*(-velocity_value + node.bias)
temp = node.normal_impulse
node.normal_impulse = max(temp + normal_impulse, 0)
normal_impulse = node.normal_impulse - temp
impulse = normal_impulse*normal
referenced_object.add_impulse(-impulse, node.r1)
incident_object.add_impulse(impulse, node.r2)
velocity = incident_object.vector + cross(incident_object.angularspeed, node.r2) - cross(
referenced_object.angularspeed, node.r1) - referenced_object.vector
tangent = cross(normal, 1)
tangent_velocity_value = velocity.dot(tangent)
tangent_impulse_value = node.mass_tangent*(-tangent_velocity_value)
friction = node.friction
max_tangent_value = friction * node.normal_impulse
old_tangent_impulse = node.tangent_impulse
node.tangent_impulse = clamp(old_tangent_impulse + tangent_impulse_value, -max_tangent_value, max_tangent_value)
tangent_impulse_value = node.tangent_impulse-old_tangent_impulse
end_tangent_impulse = tangent_impulse_value * tangent
referenced_object.add_impulse(-end_tangent_impulse, node.r1)
incident_object.add_impulse(end_tangent_impulse, node.r2)
self.cashed_data_old = self.cashed_data_new
