numpy Spring physics使用Python应用于四元数

iaqfqrcu  于 2023-10-19  发布在  Spring
关注(0)|答案(3)|浏览(91)

我想用python创建一个简单的物理系统,它以类似于velocity/position的方式在quaternions上工作。它的主要目标是模拟一个对象被拖动,并试图赶上另一个随着时间的推移。模拟使用3个变量:k:Spring常数,d:阻尼系数,以及m:拖动对象的质量。
使用经典的EQUIPMENT积分,我可以通过以下方式求解位置:

import numpy as np
from numpy.core.umath_tests import inner1d

# init simulation values
dt = 1./30. # delta t @ 30fps
k = 0.5 # Spring constant
d = 0.1 # Damping
m = 0.1 # Mass

p_ = np.array([0,0,0]) # initial position of the dragged object
p  = np.array([1,2,0]) # position to catch up to, in real life this value could change over time
v  = np.array([0,0,0]) # velocity buffer (init speed is assumed to be 0)

# Euler Integration
n = 200 # loop over 200 times just to see the values converge
for i in xrange(n):
    x  = (p-p_)
    F  = (k*x - d*v) / m # spring force
    v  = v + F * dt # update velocity
    p_ = p_ + v * dt # update dragged position

    print p_ # values oscillate and eventually stabilize to p

这对位置非常有用。通过改变kdm,我可以得到一个更快/更重的结果,总的来说我对它的感觉很满意:

现在我想对quaternions做同样的事情。因为我没有使用quaternions进行物理处理的经验,所以我走了一条简单的道路,应用了相同的函数,并进行了一些修改来处理quaternion翻转和归一化。

# quaternion expressed as x,y,z,w
q_ = np.array([0., 0., 0., 1.]) # initial quaternion of the dragged object
q  = np.array([0.382683, 0., 0., 0.92388]) # quaternion to catch up to (equivalent to xyz euler rotation of 45,0,0 degrees)
v  = np.array([0.,0.,0.,0.]) # angular velocity buffer (init speed is assumed to be 0)

# Euler Integration
n = 200
for i in xrange(n):
    
    # In a real life use case, q varies over time and q_ tries to catch up to it.
    # Test to see if q and q_ are flipped with a dot product (innder1d).
    # If so, negate q
    if inner1d(q,q_) < 0:
        q = np.negative(q)
    
    x  = (q-q_)
    F  = (k*x - d*v) / m # spring force
    v  = v + F * dt # update velocity
    q_ = q_ + v * dt # update dragged quaternion
    q_ /= inner1d(q_,q_) # normalize
    
    print q_ # values oscillate and eventually stabilize to q

令我惊讶的是,它给了我非常好的结果!

因为我跟着直觉走,我确信我的解决方案是有缺陷的(比如q和q_是对立的),而且有一个正确/更好的方法来实现我想要的。

问题:

quaternions上模拟Spring力的正确方法是什么,该方法至少要考虑拖动对象的mass,Springstiffnessdamping factor
实际的python代码将非常感谢,因为我有很大的困难阅读博士论文:)。同样对于quaternion的操作,我通常使用Christoph Gohlke's excellent library,但请在您的答案中随意使用任何其他操作。

sh7euo9m

sh7euo9m1#

“更好”这个词其实很主观
你就像是谋杀了四元数的概念,因为你的步长和位移都很小。根据您的应用程序,这实际上可能是可以的(游戏引擎经常利用这样的技巧来使实时计算更容易),但如果您的目标是准确性,或者您希望增加步长并且不会得到不稳定的结果,则需要使用四元数。
正如@z0r在评论中所解释的,由于四元数通过乘法来转换旋转,它们之间的“差异”是乘法逆-基本上是四元数除法。

qinv = quaternion_inverse(q)  # Using Gohlke's package 
x = quaternion_multiply(q_, qinv)

现在,就像对于小thetatheta =~ sin(theta)一样,只要差很小,这个x就不会与减法的结果有很大的不同。像这样滥用“小Angular 定理”经常在所有类型的模拟中使用,但重要的是要知道何时打破它们以及它对模型的限制。
加速度和速度仍然增加,所以我认为这仍然有效:

F  = (k*x - d*v) / m 
v  = v + F * dt

组成单位旋转

q_ = quaternion_multiply(q_, unit_vector(v * dt)) # update dragged quaternion

再次,对于小Angular (即,dt与速度相比很小),和与积非常接近。
然后,如有必要,像以前一样进行标准化。

q_ = unit_vector(q_)

我认为这应该工作,但会比你以前的版本慢一点,可能会有非常相似的结果。

yfwxisqw

yfwxisqw2#

所以我已经在这个问题上工作了很长一段时间,虽然我没有python的解决方案,但我确实有一个在luau中的解决方案。数学应该很容易从一种语言转换到另一种语言,但是我不能解释质量,但是你应该能够只用速度/阻尼来实现相同的行为。
关键是将当前旋转存储为四元数,将角速度存储为旋转矢量(即,紧凑轴角),因为这将允许角速度超过180度/秒。
我在Github上有完整的源代码,并附带了一个documentation站点。
但是,作为总结,您可以执行以下操作:
创建四元数Spring对象,该对象应具有以下字段:

  • clock -返回当前unix时间的函数,单位为ms
  • time -存储上次更新的unix时间
  • rotation -当前的旋转,作为四元数
  • velocity -Spring的速度,表示为rotation vector
  • target -Spring的目标旋转(作为四元数)
  • 阻尼-Spring的阻尼作为一个数字
  • speed -Spring的速度作为一个数字
local QuaternionSpring = {_type = "QuaternionSpring"}

function QuaternionSpring.new(initial: Quaternion, damping: number, speed: number, clock: () -> number)
    initial = initial or Quaternion.identity
    damping = damping or 1
    speed = speed or 1
    clock = clock or os.clock
    
    return setmetatable({
        _clock = clock;
        _time = clock();
        _position = initial;
        _velocity = Vector3.new();
        _target = initial;
        _damping = damping;
        _speed = speed;
    }, QuaternionSpring)
end

然后你需要创建一个函数来处理字段何时被索引以获取值(有些语言提供这个功能,有些没有,所以根据需要调整),也就是getter函数。
首先,你需要检查索引是否是一个方法调用,如果是,返回函数,然后对于位置索引,它只是通过调用更新Spring方法返回当前时钟时间的位置。速度是一样的。对于所有其他索引,您可以只返回存储的字段。

function QuaternionSpring:__index(index)
    if QuaternionSpring[index] then
        return QuaternionSpring[index]
    elseif index == "Position" or index == "p" then
        local position, _ = self:_positionVelocity(self._clock())
        return position
    elseif index == "Velocity" or index == "v" then
        local _, velocity = self:_positionVelocity(self._clock())
        return velocity
    elseif index == "Target" or index == "t" then
        return self._target
    elseif index == "Damping" or index == "d" then
        return self._damping
    elseif index == "Speed" or index == "s" then
        return self._speed
    elseif index == "Clock" then
        return self._clock
    else
        error(string.format(ERROR_FORMAT, tostring(index)), 2)
    end
end

然后你需要一个setter函数,它将设置字段并根据需要更新Spring。对于几乎所有的索引,这意味着你需要调用spring update函数,它将返回你应该设置位置和字段的位置和速度,除非位置和速度是被更新的字段,在这种情况下,将有问题的字段设置为给定的值,但只有在调用update之后。然后更新目标字段,并将时间字段设置为时钟给定的时间。

function QuaternionSpring:__newindex(index, value)
    local now = self._clock()
    if index == "Position" or index == "p" then
        local _, velocity = self:_positionVelocity(now)
        self._position = value
        self._velocity = velocity
        self._time = now
    elseif index == "Velocity" or index == "v" then
        local position, _ = self:_positionVelocity(now)
        self._position = position
        self._velocity = value
        self._time = now
    elseif index == "Target" or index == "t" then
        local position, velocity = self:_positionVelocity(now)
        self._position = position
        self._velocity = velocity
        self._target = value
        self._time = now
    elseif index == "Damping" or index == "d" then
        local position, velocity = self:_positionVelocity(now)
        self._position = position
        self._velocity = velocity
        self._damping = value
        self._time = now
    elseif index == "Speed" or index == "s" then
        local position, velocity = self:_positionVelocity(now)
        self._position = position
        self._velocity = velocity
        self._speed = value < 0 and 0 or value
        self._time = now
    elseif index == "Clock" then
        local position, velocity = self:_positionVelocity(now)
        self._position = position
        self._velocity = velocity
        self._clock = value
        self._time = value()
    else
        error(string.format(ERROR_FORMAT, tostring(index)), 2)
    end
end

您可能会注意到一件事,时间字段仅在设置字段时更新,而不是在检索字段时更新,这对Spring正常工作很重要。
现在是魔术,更新功能。这将计算一些增量时间相对值,然后将用于更新Spring。
更新Spring的基本结构如下:
1.计算一个pull四元数,使用某个pull值(相对于增量时间)将其从当前旋转平移到目标旋转。
1.通过将该拉力四元数与当前速度和某个推力值(相对于时间增量)积分来计算新位置。
1.计算当前旋转和目标旋转之间的差异,并将其转换为轴角格式
1.将轴Angular 格式转换为旋转矢量,并将此旋转矢量乘以某个速度推动因子(相对于时间增量)
1.通过将当前速度乘以某个速度衰减因子(与时间增量相关)来计算速度衰减
1.通过将步骤4中计算的速度矢量与步骤5中计算的衰减矢量相加来计算新的速度
1.返回新位置和新速度
这就是它的样子:

function QuaternionSpring:_positionVelocity(now)
    local currentRotation = self._position
    local currentVelocity = self._velocity
    local targetRotation = self._target
    local dampingFactor = self._damping
    local speed = self._speed
    
    local deltaTime = speed * (now - self._time)
    local dampingSquared = dampingFactor * dampingFactor
    
    local angFreq, sinTheta, cosTheta
    if dampingSquared < 1 then
        angFreq = math.sqrt(1 - dampingSquared)
        local exponential = math.exp(-dampingFactor * deltaTime) / angFreq
        cosTheta = exponential * math.cos(angFreq * deltaTime)
        sinTheta = exponential * math.sin(angFreq * deltaTime)
    elseif dampingSquared == 1 then
        angFreq = 1
        local exponential = math.exp(-dampingFactor * deltaTime)
        cosTheta, sinTheta = exponential, exponential * deltaTime
    else
        angFreq = math.sqrt(dampingSquared - 1)
        local angFreq2 = 2 * angFreq
        local u = math.exp((-dampingFactor + angFreq) * deltaTime) / angFreq2
        local v = math.exp((-dampingFactor - angFreq) * deltaTime) / angFreq2
        cosTheta, sinTheta = u + v, u - v
    end
    
    local pullToTarget = 1 - (angFreq * cosTheta + dampingFactor * sinTheta)
    local velPosPush = sinTheta / speed
    local velPushRate = speed * sinTheta
    local velocityDecay = angFreq * cosTheta - dampingFactor * sinTheta
    
    local posQuat = currentRotation:Slerp(targetRotation, pullToTarget)
    local newPosition = posQuat:Integrate(currentVelocity, velPosPush)
    
    local difQuat = currentRotation:Difference(targetRotation)
    local axis, angle = difQuat:ToAxisAngle()
    local velPush = (axis * angle) * velPushRate
    local velDecay = currentVelocity * velocityDecay
    
    local newVelocity = velPush + velDecay
    
    return newPosition, newVelocity
end

关于其中一些具体实现的说明:
Slerp:

local function Slerp(q0: Quaternion, q1: Quaternion, alpha: number): Quaternion
    q0 = Normalize(q0)
    q1 = Normalize(q1)

    local dot = Dot(q0, q1)

    if dot < 0 then
        q0 = unm(q0)
        dot = -dot
    end

    if dot >= 1 then
        return Normalize(Add(q0, Mul(Sub(q1, q0), alpha)))
    end

    local theta0 = math.acos(dot)
    local sinTheta0 = math.sin(theta0)

    local theta = theta0 * alpha
    local sinTheta = math.sin(theta)

    local s0 = math.cos(theta) - dot * sinTheta / sinTheta0
    local s1 = sinTheta / sinTheta0
    return Normalize(Add(Mul(s0, q0), Mul(s1, q1)))
end

整合:

local function Integrate(q0: Quaternion, rate: Vector3, timestep: number): Quaternion
    q0 = Normalize(q0)
    
    local rotationVector = (rate * timestep)
    local rotationMag = rotationVector.Magnitude
    if rotationMag > 0 then
        local axis = rotationVector / rotationMag
        local angle = rotationMag
        local q1 = fromAxisAngle(axis, angle)
        return Normalize(Mul(q0, q1))
    else
        return q0
    end
end

差异:

local function Difference(q0: Quaternion, q1: Quaternion): Quaternion
    if Dot(q0, q1) < 0 then
        q0 = unm(q0)
    end
    return Mul(Inverse(q0), q1)
end

我知道这个答案很长,但希望这对将来想知道四元数Spring如何工作的人有所帮助。我知道这个问题很老了,但这个问题在过去的3个月里一直是我最大的敌人,我不想让其他人陷入同样的境地:)

zzzyeukh

zzzyeukh3#

对于“在四元数上模拟Spring力的正确方法是什么”这个问题,答案是正确地写下势能。
1.四元数通过操作为您提供对象的方向

orientation = vector_part(q_ r q_*)

其中星星表示共轭,r是一个固定的方向(例如,“沿沿着z的单位向量”,它必须在你的系统中对所有对象都是唯一的)。假定q_、r和q_* 的乘法是“四元数乘法”。
1.用这个方向定义能量函数,

energy = dot_product(orientation, unit_z)

1.取能量对四元数的“负导数”,你就得到了作用在系统上的力。
你现在的代码做了一个“四元数空间中的阻尼振荡器”,这可能是解决你问题的一个很好的方法,但它不是一个物体上的Spring力:-)
PS:太长的评论,我希望它有帮助。PS2:我没有直接使用代码来解决上面的问题,因为(i)我发现阅读上面的库文档并不容易,(ii)问题的第一部分是做数学/物理。

相关问题