vpython_spinning_and_flipping_four_points_six_springs

# vpython_spinning_and_flipping_four_points_six_springs_3.py
# 31_12_21
# ...
# 02_03_22

import numpy as np
from functools import partial
import sys
from vpython import *

'''

 REFERENCES:

 Ideas for a rigid body made from springs from:
   Rhett Allain's Physics Explained videos

 Shane Ross - Ross Dynamics Lab videos

 Analytical Mechanics of Space Systems (Fourth Edition 2018)
 by Hanspeter Schaub & John L. Junkins

 Classical Mechanics by John R. Taylor (2005)

 An Introduction to the Coriolis Force
 by Henry M. Stommel & Dennis W. Moore (1989)

  Stommel's Law (p.62):
   ⸺⸺⸺⸺⸺⸺⸺⸺
 "Particles that are not at rest in the rotating reference frame...
   find themselves acted upon by the Coriolis forces."

  ...and, unfortunately, by diagonal spring forces.


  Taylor p. 348

  Fcor = 2mṙ ⨯ 𝛚  = 2mv ⨯ 𝛚

  "...where v is object's velocity relative to the rotating frame"

'''

def main():

    scene.width, scene.height = 1000, 800
    scene.background = color.black
    scene.align = 'left'
    scene.autoscale = False
    scene.autozoom = False
    scene.userspin = False
    scene.userzoom = False
    scene.userpan = False
    scene.resizable = False
    scene.center = vec(0.0, 0.0, 0.0)
    scene.range = 1.3

    PAUSED_AT_START = False

    RATE = 100

    DELTA_T = 0.01

    SPRING_CONSTANT = 2000

    GRAPH = False

    DIAGONAL_SPRINGS = False

    APPARENT_VELOCITIES = 0

    # SET THE "ROTATING REFERENCE FRAME"
    # TODO: SHOULD THE ROTATING REFERENCE FRAME BE UNPERTURBED?

    SELECT = 4

    if SELECT == 1:
        BANNER = '(1) Rotate about X with Y and Z perturbation'
        𝛀 = vec(3.0, 0.01, 0.01)

    if SELECT == 2:
        BANNER = '(2) Rotate about X with no perturbation'
        𝛀 = vec(3.0, 0.0, 0.0)

    if SELECT == 3:
        BANNER = '(3) Rotate about Y with X and Z perturbation'
        𝛀 = vec(0.01, 3.0, 0.01)

    if SELECT == 4:
        BANNER = '(4) Rotate about Z with X and Y perturbation'
        𝛀 = vec(0.01, 0.01, 3.0)


    '''
         A       D
          ⬤─────⬤
          │     │
          │     │
          │     │
          │     │
          ⬤─────⬤
         B       C
    '''

    posA = vec(-0.3, 0.0,  0.5)
    posB = vec(-0.3, 0.0, -0.5)
    posC = vec( 0.3, 0.0, -0.5)
    posD = vec( 0.3, 0.0,  0.5)

    mA = 0.4
    mB = 0.4
    mC = 0.4
    mD = 0.4

    # Center of Mass: CM = ∑mr / M  # Taylor p. 87
    M = mA + mB + mC + mD
    CM = (mA*posA + mB*posB + mC*posC + mD*posD) / M
    # ~ label(text='CM', pos=CM, height=14, box=0, opacity=0)


    pointA = Sphere(pos=posA, color=color.green)
    pointB = Sphere(pos=posB, color=color.red)
    pointC = Sphere(pos=posC, color=color.blue)
    pointD = Sphere(pos=posD, color=color.yellow)

    pointA.m, pointB.m, pointC.m, pointD.m = mA, mB, mC, mD

    pointA.lastpos = vec(pointA.pos)
    pointB.lastpos = vec(pointB.pos)
    pointC.lastpos = vec(pointC.pos)
    pointD.lastpos = vec(pointD.pos)

    # VELOCITIES
    # v = 𝝎  ⨯ r   # Taylor p. 338
    pointA.v = cross(𝛀, pointA.pos)
    pointB.v = cross(𝛀, pointB.pos)
    pointC.v = cross(𝛀, pointC.pos)
    pointD.v = cross(𝛀, pointD.pos)

    # MOMENTA
    # p = mv   # Taylor p. 14
    pointA.p = pointA.m * pointA.v
    pointB.p = pointB.m * pointB.v
    pointC.p = pointC.m * pointC.v
    pointD.p = pointD.m * pointD.v

    # THE VECTORS
    rAB = pointB.pos - pointA.pos
    rBC = pointC.pos - pointB.pos
    rCD = pointD.pos - pointC.pos
    rDA = pointA.pos - pointD.pos
    rBD = pointD.pos - pointB.pos
    rAC = pointC.pos - pointA.pos

    # THE SPRINGS
    spring1 = Spring(pos=posA, axis=rAB)
    spring2 = Spring(pos=posB, axis=rBC)
    spring3 = Spring(pos=posC, axis=rCD)
    spring4 = Spring(pos=posD, axis=rDA)
    if DIAGONAL_SPRINGS:
        spring5 = Spring(pos=posC, axis=rBD)
        spring6 = Spring(pos=posD, axis=rAC)


    # THE "RELAXED" LENGTH OF THE SPRINGS (L0)
    rAB_L0 = rAB.mag
    rBC_L0 = rBC.mag
    rCD_L0 = rCD.mag
    rDA_L0 = rDA.mag
    rBD_L0 = rBD.mag
    rAC_L0 = rAC.mag


    def pause_run():
        g.paused = not g.paused
        pause.text='  RUN   ' if g.paused else 'PAUSE'
    scene.append_to_caption(' '*25)
    pause = button(bind=pause_run)
    g.paused = not PAUSED_AT_START
    pause_run()


    def key(e):
        # can't use space or enter because focus of button changes
        if e.key in ['p', 'P']: pause_run()
    scene.bind('keydown', key)


    TEXT = 'CENTRIFUGAL FORCE ACTIVE'
    t1 = Label(text=TEXT, pos=vec(-0.6, -1.1, 0))
    g.centrifugal_flag = True
    def centrifugal_button():
        g.centrifugal_flag = not g.centrifugal_flag
        t1.visible = g.centrifugal_flag
    scene.append_to_caption(' '*5)
    centrifugal_button()
    text = 'CENTRIFUGAL FORCE'
    button(text=text, bind=centrifugal_button)

    TEXT = 'CORIOLIS FORCE ACTIVE'
    t2 = Label(text=TEXT, pos=vec(0.6, -1.1, 0))
    g.coriolis_flag = True
    def coriolis_button():
        g.coriolis_flag = not g.coriolis_flag
        t2.visible = g.coriolis_flag
    scene.append_to_caption(' '*5)
    coriolis_button()
    button(text='CORIOLIS FORCE', bind=coriolis_button)


    def both():
        if not (g.coriolis_flag and g.centrifugal_flag):
            g.centrifugal_flag, g.coriolis_flag = 1, 1
            t1.visible, t2.visible = 1, 1
        else:
            g.centrifugal_flag, g.coriolis_flag = 0, 0
            t1.visible, t2.visible = 0, 0
    scene.append_to_caption(' '*5)
    button(text='TOGGLE BOTH FORCES', bind=both)


    if GRAPH:
        scene.append_to_caption('\n'*4)
        TEXT = ''
        GRAPH = graph(width=800, height=500, align='right',
                      xmin = 0., xmax = 1.,
                      ymin = 0., ymax = 8.,
                      title=TEXT, )
        pen1 = gcurve(color=color.red)
        pen2 = gcurve(color=color.green)
        pen3 = gcurve(color=color.blue)

    t3 = Label(text=BANNER, pos=vec(0.0, 1.1, 0.0), height=26)

    t = 0.0
    dt = DELTA_T
    k = SPRING_CONSTANT

    while 1:

        rate(RATE)

        if g.paused: continue

        # UPDATE VECTORS
        rAB = pointB.pos - pointA.pos
        rBC = pointC.pos - pointB.pos
        rCD = pointD.pos - pointC.pos
        rDA = pointA.pos - pointD.pos
        rBD = pointD.pos - pointB.pos
        rAC = pointC.pos - pointA.pos

        # UPDATE SPRING FORCE OF ONE MASS ON ANOTHER
        # Fs = -k * (‖L‖ - L0) * L.hat
        FAB = -k * (rAB.mag - rAB_L0) * rAB.hat
        FBC = -k * (rBC.mag - rBC_L0) * rBC.hat
        FCD = -k * (rCD.mag - rCD_L0) * rCD.hat
        FDA = -k * (rDA.mag - rDA_L0) * rDA.hat
        if DIAGONAL_SPRINGS:
            FBD = -k * (rBD.mag - rBD_L0) * rBD.hat
            FAC = -k * (rAC.mag - rAC_L0) * rAC.hat

        # SUMMATION OF FORCES ON EACH MASS
        if DIAGONAL_SPRINGS:
            FA = FDA - FAB - FAC
            FB = FAB - FBC - FBD
            FC = FBC - FCD + FAC
            FD = FCD - FDA + FBD
        else:
            FA = FDA - FAB
            FB = FAB - FBC
            FC = FBC - FCD
            FD = FCD - FDA

        # UPDATE VELOCITIES
        # v = 𝝎  ⨯ r   # Taylor p. 338
        pointA.v.value = cross(𝛀, pointA.pos)
        pointB.v.value = cross(𝛀, pointB.pos)
        pointC.v.value = cross(𝛀, pointC.pos)
        pointD.v.value = cross(𝛀, pointD.pos)

        if g.coriolis_flag:

            if APPARENT_VELOCITIES:
                Av = (pointA.pos - pointA.lastpos) / dt
                Bv = (pointB.pos - pointB.lastpos) / dt
                Cv = (pointC.pos - pointC.lastpos) / dt
                Dv = (pointD.pos - pointD.lastpos) / dt

                FA += Coriolis_force(pointA.m, 𝛀, Av)
                FB += Coriolis_force(pointB.m, 𝛀, Bv)
                FC += Coriolis_force(pointC.m, 𝛀, Cv)
                FD += Coriolis_force(pointD.m, 𝛀, Dv)
            else:
                # Fcor = 2mṙ ⨯ 𝛀  = 2mv ⨯ 𝛀  # Taylor p. 348
                FA += Coriolis_force(pointA.m, 𝛀, pointA.v)
                FB += Coriolis_force(pointB.m, 𝛀, pointB.v)
                FC += Coriolis_force(pointC.m, 𝛀, pointC.v)
                FD += Coriolis_force(pointD.m, 𝛀, pointD.v)

        if g.centrifugal_flag:
            # Fcf = m(𝛀 ⨯ r) ⨯ 𝛀    # Taylor p. 344
            FA += centrifugal_force(pointA.m, 𝛀, pointA.pos)
            FB += centrifugal_force(pointB.m, 𝛀, pointB.pos)
            FC += centrifugal_force(pointC.m, 𝛀, pointC.pos)
            FD += centrifugal_force(pointD.m, 𝛀, pointD.pos)

        # UPDATE MOMENTUM OF THE MASSES
        # p2 = p1 + Fnet * 𝛥t
        pointA.p += FA * dt
        pointB.p += FB * dt
        pointC.p += FC * dt
        pointD.p += FD * dt

        # RECORD CURRENT POSITIONS
        pointA.lastpos.value = pointA.pos
        pointB.lastpos.value = pointB.pos
        pointC.lastpos.value = pointC.pos
        pointD.lastpos.value = pointD.pos

        # UPDATE POSITIONS OF MASSES
        # r2 = r1 + p/m * 𝛥t
        pointA.pos += pointA.p / pointA.m * dt
        pointB.pos += pointB.p / pointB.m * dt
        pointC.pos += pointC.p / pointC.m * dt
        pointD.pos += pointD.p / pointD.m * dt

        # UPDATE POSITIONS AND LENGTHS OF SPRINGS
        spring1.pos = pointA.pos; spring1.axis = rAB
        spring2.pos = pointB.pos; spring2.axis = rBC
        spring3.pos = pointC.pos; spring3.axis = rCD
        spring4.pos = pointD.pos; spring4.axis = rDA
        if DIAGONAL_SPRINGS:
            spring5.pos = pointB.pos; spring5.axis = rBD
            spring6.pos = pointA.pos; spring6.axis = rAC

        if GRAPH:
            if t > GRAPH.xmax:
                d = t - GRAPH.xmax
                GRAPH.xmin += d
                GRAPH.xmax += d
            # ~ pen1.plot(t, (pointA.pos-pointB.pos).mag)
            # ~ pen1.plot(t, pointA.v.mag)

        t += dt


def centrifugal_force(m, 𝛚, r):

    """Find the vector of the centrifugal force given
    mass (m), angular velocity vector (𝛚), and position vector (r).

    Fcf = m(𝛚 ⨯ r) ⨯ 𝛚    # Taylor p. 344  """

    return m * cross(cross(𝛚, r), 𝛚)


def Coriolis_force(m, 𝛚, v):

    """Find the vector of the Coriolis force given
    mass (m), angular velocity vector (𝛚), and velocity vector (v).

    Fcor = 2mṙ ⨯ 𝛚  = 2mv ⨯ 𝛚    # Taylor p. 348  """

    return cross(2.0*m*v, 𝛚)


def angle_between(u, v):
    """
Returns the angle between two numpy vectors in radians (0 to pi)

A nugget from Mindless.pdf (section 12), by William Kahan
(https://people.eecs.berkeley.edu/~wkahan/Mindless.pdf)
Used in Brandon Rhodes' Skyfield (functions.py)

Prof. W. Kahan:
"Here is a better formula less well known than it deserves:
     ∠(x,y) := 2arctan(‖x.‖y‖ - ‖x‖.y‖/‖x.‖y‖ + ‖x‖.y‖)"

∠(x,y) = 2*atan( ‖( x*‖y‖ - ‖x‖*y )‖
                 -------------------
                 ‖( x*‖y‖ + ‖x‖*y )‖ )

∠(x,y) = 2*atan( mag( x*mag(y) - y*mag(x) )
                 ------------------------
                 mag( x*mag(y) + y*mag(x) ) )

∠(x,y) = 2*atan2(mag(x*mag(y) - y*mag(x)), mag(x*mag(y) + y*mag(x)))

notes:
mag(v) = length_of(v) = ‖v‖ = sqrt(v.dot(v)) # v = np.array([x,y,z])
print('\u2016')
‖
print('\u2220')
∠

    """
    # from math import atan2, sqrt

    a = u * sqrt(v.dot(v))
    b = v * sqrt(u.dot(u))
    y = a - b
    x = a + b
    return 2.0 * atan2( sqrt(y.dot(y)), sqrt(x.dot(x)) )



Sphere = partial(sphere, radius=0.06)


brass = vec(0.71, 0.65, 0.26)
Spring = partial(helix, radius=.015, coils=45, thickness=.004, color=brass)


Label = partial(label,
                height = 22,      # default is 15 pixels
                box = True,       # default is True
                visible = True,   # default is True
                font = 'sans',    # 'sans', 'serif', 'monospace'
                line = False,     # default is True
                border = 10,      # default is 5 pixels
                align = 'center', # 'left', 'right', 'center'
                opacity = 0.5,    # default is 0.66
                color = vec(1,1,1),
                background = vec(.2,.2,.2),
                )

Arrow = partial(arrow, shaftwidth=0.04, color=color.blue)


class g: ...


if __name__ == '__main__':
    sys.exit(main())