Skip to content

Instantly share code, notes, and snippets.

@BananaHemic
Created August 2, 2020 07:12
Show Gist options
  • Save BananaHemic/9c43939b143682ebdb289029dec5faa0 to your computer and use it in GitHub Desktop.
Save BananaHemic/9c43939b143682ebdb289029dec5faa0 to your computer and use it in GitHub Desktop.
// I don't dance now, I make muscle moves...
// Configurable Params
MaxMoveF = 4000
MaxHoldF = 1.334 * MaxMoveF
MaxTorque = 500
MaxHoldTorque = 1.334 * MaxTorque
MaxMoveSpeed = 4.5 // m/s
HillParamA = 0.25 // Coefficient of shortening heat in Hill's muscle model
SetAngularDrag(0)
dtSqr = fixedDeltaTime * fixedDeltaTime
// Load the relevant information about the object and where our hand is
mass = GetMass()
weight = Vector3(0, mass * 9.81, 0)
// Fyi, the inertia tensor includes the mass
inertiaRaw = GetInertiaTensor()
inertiaRot = GetInertiaTensorRotation()
// The inertia tensor from Unity includes the mass
inertia = inertiaRot * inertiaRaw
SetBounciness(0.1)
didAddScript = false
zero = Vector3(0,0,0)
yLine = Line(Vector3(0,0,0), Vector3(0,1,0), "yellow")
gLine = Line(Vector3(0,0,0), Vector3(0,1,0), "green")
cLine = Line(Vector3(0,0,0), Vector3(0,1,0), "cyan")
mLine = Line(Vector3(0,0,0), Vector3(0,1,0), "magenta")
handOffset = Vector3(0, 0.2, 0)
anchorPos = zero
OnFixedUpdate = function()
if GetGrabbingUser() == null then
if globals.didAddScript then
RemoveScript("Configurable Joint")
globals.didAddScript = false
end if
return
end if
if not globals.didAddScript then
AddScript("Configurable Joint")
anchor = GetGrabRelativePosRot()
//print("anchor pos " + anchor.position)
SetAnchorPoint(anchor.position)
globals.anchorPos = anchor.position
globals.didAddScript = true
end if
// Info about the ideal place for us to be
targPosRot = GetInstantGrabbedPosRotVel()
grabPosRot = GetGrabRelativePosRot()
handPosRot = GetRealHandPosRot()
targAnchorOffset = targPosRot.rotation * globals.anchorPos
targAnchorPos = targPosRot.position + targAnchorOffset
SetTargetPosRot(zero, targPosRot.rotation)
ourAnchorPos = position + rotation * globals.anchorPos
// Update where the other end of the spring is
SetConnectedAnchor(targAnchorPos)
//SetPositionDriveSpring(10000, 10000, 10000)
SetPositionDriveDamper(1, 1, 1)
//SetRotationSlerpDrive(2000)
/// POSITION
delPos = targAnchorPos - ourAnchorPos
gLine.Update(ourAnchorPos, delPos)
posF = mass * (delPos - velocity * fixedDeltaTime) / dtSqr
posFMag = posF.Magnitude()
vMag = velocity.Magnitude
// Clamp to the maximum that is physiologically allowed
// If we're applying a force against the velocity, then
// we have slightly more force to give
cLine.Update(position, posF.Normalized() / 5)
yLine.Update(position, velocity)
againstV = vMag < 0.1 or dot(posF, velocity) < -0.3
maxMag = MaxMoveF
if againstV then
maxMag = MaxHoldF
//SetColor("red")
else
//SetColor("green")
// Normalize the speed by the max speed
v = abs(vMag) / MaxMoveSpeed
if v >= 1 then
// we're moving at the max speed
// so we can't apply any force
maxMag = 0
else
// Normalized Hill model, so a=b
f = HillParamA * (1 + HillParamA) / (v + HillParamA) - HillParamA
// Get the non-normalized force
maxMag = f * MaxMoveF
end if
end if
delPosMag = delPos.Magnitude()
posFMag = clamp(posFMag, -maxMag, maxMag)
// f = -kx - dv
posSpringK = posFMag / delPosMag
SetPositionDriveSpring(posSpringK, posSpringK, posSpringK)
/// ROTATION
targAngVel = targPosRot.angularVelocity
// Turn the angular velocities into quaternions
angVelQ = Quaternion(angularVelocity.Normalized, angularVelocity.Magnitude)
tAngVelQ = Quaternion(targAngVel.Normalized, targAngVel.Magnitude)
// How much the rotation will change by next frame
angleStep = Quaternion(angularVelocity.Normalized, fixedDeltaTime * angularVelocity.Magnitude)
tAngleStep = Quaternion(targAngVel.Normalized, fixedDeltaTime * targAngVel.Magnitude)
// What our rotation will be next frame
nextRot = angleStep * rotation
targNextRot = tAngleStep * targPosRot.rotation
// Get the rotation that we're planning to correct
// delRot is the angle from where the object is, to where
// we want it to be
delRot = 0
if dot(targNextRot, nextRot) >= 0 then
delRot = targNextRot * nextRot.inv()
else
flippedNextRot = -nextRot
delRot = targNextRot * flippedNextRot.inv()
end if
angleAxis = delRot.ToAngleAxis()
delRad = radians(angleAxis.angle)
//print("del degrees " + angleAxis.angle)
// Get the torque needed to correct the rotation
accel = delRad / dtSqr
angAccel = angleAxis.axis * accel
posTorque = rotation * ((rotation.inv() * angAccel).ComponentMul(inertia))
// Get the torque needed to correct the angular velocity
delAngVel = targAngVel - angularVelocity
velAngAccel = delAngVel / fixedDeltaTime
velTorque = rotation * ((rotation.inv() * velAngAccel).ComponentMul(inertia))
// Use the delta angle to find the weight for correcting angle/angularVelocity
// 0 means just correct angle, >= 1 means correct angular velocity
//delAngle = rotation.inv() * (angleAxis.angle * angleAxis.axis)
//wAngle = (abs(delAngle) - SmallAngleVec).ComponentDiv(LSAngleVec)
wTorque = posTorque
// We can apply a larger torque if we're appying a force against
// the velocity
rotAgainstVel = angularVelocity.Magnitude < 0.1 // Rad/s
rotAgainstVel = rotAgainstVel or dot(angularVelocity, wTorque) < 0.01
clampT = MaxTorque
if rotAgainstVel then
clampT = MaxHoldTorque
//mLine.Update(Vector3(1, 2, 0), Vector3(0, -1, 0))
//SetColor("red")
else
//SetColor("green")
//mLine.Update(Vector3(1, 2, 0), Vector3(0, 1, 0))
end if
// Clamp the torque
rotTMag = wTorque.Magnitude()
rotTMag = clamp(abs(rotTMag), 0, clampT)
//print("rotF " + rotFMag)
// Apply the torque via the spring parameter
rotSpringK = 0
if delRad > 0.01 then
rotSpringK = rotTMag / delRad
rotSpringK = clamp(rotSpringK, 0, 20000)
end if
SetRotationSlerpDrive(rotSpringK)
availT = clampT - rotTMag
damper = 0
if availT < 0.1 then
SetRotationSlerpDamper(0)
else
delAngVelMag = (angularVelocity - targAngVel).Magnitude()
if delAngVelMag > 0.01 then
damper = availT / delAngVelMag
//print("delAngle " + degrees(delRad) + " delV " + delAngVelMag + " availT " + availT)
damper = clamp(damper, 0, 50)
end if
springAngVel = rotation.inv() * targAngVel
springAngVel = -springAngVel
SetTargetAngularVelocity(springAngVel)
SetRotationSlerpDamper(damper)
//print("rot damper: " + damper)
end if
//print("rot springK " + rotSpringK + " d " + damper)
end function
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment