MetaBox/Scripts/Animation/springmagic/springMath.py

import math
import pymel.core as pm
import pymel.core.datatypes as dt


def sigmoid(x):
    return 1 / (1 + math.exp(-x))


def distance(a, b):
    return (b - a).length()


def lerp_vec(a, b, t):
    return (a * (1 - t)) + (b * t)


def dist_to_plane(pt, n, d):
    return n.dot(pt) - (d / n.dot(n))


def dist_to_line(a, b, p):
    ap = p - a
    ab = b - a
    result = a + ((ap.dot(ab) / ab.dot(ab)) * ab)
    return distance(result, p)


def is_same_side_of_plane(pt, test_pt, n, d):
    d1 = math.copysign(1, dist_to_plane(pt, n, d))
    d2 = math.copysign(1, dist_to_plane(test_pt, n, d))

    # print(pt, test_pt, d1, d2)
    return d1 * d2 == 1.0


def proj_pt_to_plane(pt, n, d):
    t = n.dot(pt) - d
    return (pt - (n * t))


def pt_in_sphere(pt, c, r):
    return (pt - c).length() <= r


def pt_in_cylinder(pt, p, q, r):
    n = (q - p).normal()
    d = n.dot(p)

    if not is_same_side_of_plane(pt, (p + q) / 2.0, n, d):
        return False

    n = (q - p).normal()
    d = n.dot(q)

    if not is_same_side_of_plane(pt, (p + q) / 2.0, n, d):
        return False

    proj_pt = proj_pt_to_plane(pt, n, d)
    # logging("proj_pt", proj_pt)
    # logging("q", q)
    # logging("distance(proj_pt, q)", distance(proj_pt, q))

    return distance(proj_pt, q) <= r


def segment_sphere_isect(sa, sb, c, r):
    NotFound = (False, None)

    p = sa
    d = (sb - sa).normal()

    m = p - c
    b = m.dot(d)
    c = m.dot(m) - r * r

    if c > 0.0 and b > 0.0:
        return NotFound

    discr = b * b - c
    if discr < 0.0:
        return NotFound

    t = -b - math.sqrt(discr)
    if t < 0.0:
        return NotFound

    dist = distance(sa, sb)
    q = p + d * t
    return ((t >= 0 and t <= dist), q)


def segment_cylinder_isect(sa, sb, p, q, r):
    SM_EPSILON = 1e-6
    d = q - p
    m = sa - p
    n = sb - sa
    md = m.dot(d)
    nd = n.dot(d)
    dd = d.dot(d)

    NotFound = (False, None)
    if md < 0 and md + nd < 0:
        return NotFound

    if md > dd and md + nd > dd:
        return NotFound

    nn = n.dot(n)
    mn = m.dot(n)

    a = dd * nn - nd * nd
    k = m.dot(m) - r * r
    c = dd * k - md * md

    if abs(a) < SM_EPSILON:
        if c > 0:
            return NotFound
        if md < 0:
            t = -mn / nn
        elif md > dd:
            t = (nd - mn) / nn
        else:
            t = 0
        return (True, lerp_vec(sa, sb, t))

    b = dd * mn - nd * md
    discr = b * b - a * c
    if discr < 0:
        return NotFound

    t = (-b - math.sqrt(discr)) / a
    if t < 0.0 or t > 1.0:
        return NotFound
    if (md + t * nd < 0.0):
        if nd <= 0.0:
            return NotFound
        t = -md / nd
        return (k + 2 * t * (mn + t * nn) <= 0.0, lerp_vec(sa, sb, t))
    elif md + t * nd > dd:
        if nd >= 0.0:
            return NotFound
        t = (dd - md) / nd
        return (k + dd - 2 * md + t * (2 * (mn - nd) + t * nn) <= 0.0, lerp_vec(sa, sb, t))

    return (True, lerp_vec(sa, sb, t))


def pt_in_capsule(pt, p, q, r):
    return pt_in_cylinder(pt, p, q, r) or pt_in_sphere(pt, p, r) or pt_in_sphere(pt, q, r)


def segment_capsule_isect(sa, sb, p, q, r):
    # sa = dt.Vector()
    #    ray start point pos vector
    # sb = dt.Vector()
    #    ray end point pos vector
    # p = dt.Vector()
    #    capsle one sphere tip pos
    # q = dt.Vector()
    #    capsle another sphere tip pos
    # r = float
    #    radio of capsle sphere

    if pt_in_capsule(sa, p, q, r):
        if pt_in_capsule(sb, p, q, r):
            # both inside. extend sb to get intersection
            newb = sa + (sb - sa).normal() * 200.0
            sa, sb = newb, sa
        else:
            sb, sa = sa, sb

    # d = (sb - sa).normal()

    i1 = segment_sphere_isect(sa, sb, p, r)
    i2 = segment_sphere_isect(sa, sb, q, r)
    i3 = segment_cylinder_isect(sa, sb, p, q, r)

    dist = float('inf')
    closest_pt = None
    hit = False
    hitCylinder = False

    for i in [i1, i2, i3]:

        if i[0]:
            hit = True
            pt = i[1]

            if distance(sa, pt) < dist:
                closest_pt = pt

            dist = min(dist, distance(sa, pt))
            # draw_locator(i1[2], 'i1')

    return (hit, closest_pt, hitCylinder)


def checkCollision(cur_pos, pre_pos, capsuleLst, isRevert):
    # calculate collision with all the capsule in scene
    if isRevert:
        sa = cur_pos
        sb = pre_pos
    else:
        sb = cur_pos
        sa = pre_pos

    isHited = False
    closest_pt_dict = {}

    for obj in capsuleLst:
        objChildren = pm.listRelatives(obj, children=1, type='transform')
        p = objChildren[0].getTranslation(space='world')
        q = objChildren[1].getTranslation(space='world')
        r = obj.getAttr('scaleZ') * 1

        hit, closest_pt, hitCylinder = segment_capsule_isect(sa, sb, p, q, r)

        if hit:
            isHited = True
            closest_pt_dict[obj.name()] = [obj, closest_pt]
            # drawDebug_box(closest_pt)

    if isHited:
        pt_length = 9999
        closest_pt = None
        col_obj = None

        for pt in closest_pt_dict.keys():
            lLength = (closest_pt_dict[pt][1] - pre_pos).length()

            if lLength < pt_length:
                pt_length = lLength
                closest_pt = closest_pt_dict[pt][1]
                col_obj = closest_pt_dict[pt][0]

        # return col pt and col_body speed
        return closest_pt, col_obj, hitCylinder
    else:
        return None, None, None


def ckeckPointInTri(pos, pa, pb, pc):
    ra = math.acos(((pa - pos).normal()).dot((pb - pos).normal()))
    ra = dt.degrees(ra)
    rb = math.acos(((pb - pos).normal()).dot((pc - pos).normal()))
    rb = dt.degrees(rb)
    rc = math.acos(((pc - pos).normal()).dot((pa - pos).normal()))
    rc = dt.degrees(rc)

    return (abs(ra + rb + rc) > 359)


def getVertexPositions(obj):
    vertex_positions_list = []

    for vertex in obj.vtx:
        vertex_positions_list.append(vertex.getPosition(space='world'))

    return vertex_positions_list


def checkPlaneCollision(objPos, childPos, colPlane):

    v_coords = getVertexPositions(colPlane)

    collision_plane_matrix = pm.xform(colPlane, worldSpace=1, matrix=1, q=1)
    n = dt.Vector(collision_plane_matrix[4:7])    # Y axis direction of plane
    q = v_coords[1]
    d = n.dot(q)

    # get obj distance to plane
    toPlaneDistance = dist_to_plane(objPos, n, d)
    toPlaneDistance_child = dist_to_plane(childPos, n, d)

    # child projection position on plane
    projectPos_child = proj_pt_to_plane(childPos, n, d)

    inPlane = False

    if ckeckPointInTri(projectPos_child, v_coords[0], v_coords[1], v_coords[2]):
        inPlane = True
    elif ckeckPointInTri(projectPos_child, v_coords[3], v_coords[1], v_coords[2]):
        inPlane = True

    # bone above plane and bone child under plane and child project point on plane
    # means has collision with plane
    if (toPlaneDistance > 0) and (toPlaneDistance_child < 0) and inPlane:
        return projectPos_child
    else:
        return None