def rot_axis_quat(vector1, vector2):
""" Find the rotation (quaternion) from vector 1 to vector 2"""
vector1 = vector1.normalized()
vector2 = vector2.normalized()
cosTheta = vector1.dot(vector2)
rotationAxis = Vector((0.0, 0.0, 0.0))
if (cosTheta < -1 + 0.001):
v = Vector((0.0, 1.0, 0.0))
#Get the vector at the right angles to both
rotationAxis = vector1.cross(v)
rotationAxis = rotationAxis.normalized()
q = Quaternion()
q.w = 0.0
q.x = rotationAxis.x
q.y = rotationAxis.y
q.z = rotationAxis.z
else:
rotationAxis = vector1.cross(vector2)
s = math.sqrt((1.0 + cosTheta) * 2.0)
invs = 1 / s
q = Quaternion()
q.w = s * 0.5
q.x = rotationAxis.x * invs
q.y = rotationAxis.y * invs
q.z = rotationAxis.z * invs
return q
def quaternion_from_two_vectors(dir, up):
from io_scene_forsaken import forsaken_utils
vU1 = dir
vU2 = up
vU1.normalize()
vU2.normalize()
vAxis = vU1.cross(vU2)
fAxisMag = forsaken_utils.min(math.sqrt(vAxis.x*vAxis.x+vAxis.y*vAxis.y+vAxis.z*vAxis.z), 1.0)
fTheta = math.asin(fAxisMag)
fTheta_C = math.pi - fTheta
if vU1.dot(vU2) < 0.0:
fTheta = fTheta_C
fTheta_C = math.pi - fTheta
fEpsilon = 1e-7
if fTheta < fEpsilon:
qRot = Quaternion()
qRot.x = 0.0
qRot.y = 0.0
qRot.z = 0.0
qRot.w = 1.0
return qRot
if fTheta_C < fEpsilon:
vCP = vU1.cross(Vector((1.0, 0.0, 0.0)))
fOpposite = vCP.x*vCP.x+vCP.y*vCP.y+vCP.z*vCP.z
if fOpposite >= fEpsilon:
vAxis = vCP
else:
vAxis = Vector((0.0, 1.0, 0.0))
vAxis.normalize()
qRot = Quaternion()
s = math.sin(fTheta * 0.5)
c = math.cos(fTheta * 0.5)
qRot.x = vAxis.x * s
qRot.y = vAxis.y * s
qRot.z = vAxis.z * s
qRot.w = c
qRot.normalize()
return qRot
def get_quat(w=1.0, x=0.0, y=0.0, z=0.0):
quat = Quaternion((0, 0, 0, 0))
quat.w = w
quat.x = x
quat.y = y
quat.z = z
return quat
def decode(data, channel, scale):
scaleFactor = float(1.0)
if data.bit_count == 8:
scaleFactor = 1 / float(16)
result = [None] * channel.num_time_codes
result[0] = data.initial_value
for i, delta_block in enumerate(data.delta_blocks):
deltaScale = scale * scaleFactor * DELTA_TABLE[delta_block.block_index]
deltas = get_deltas(delta_block.delta_bytes, data.bit_count)
for j, delta in enumerate(deltas):
idx = int(i / channel.vector_len) * 16 + j + 1
if idx >= channel.num_time_codes:
break
if channel.type == 6:
# access quat as xyzw instead of wxyz
index = (delta_block.vector_index + 1) % 4
value = result[idx - 1][index] + deltaScale * delta
if result[idx] is None:
result[idx] = Quaternion()
result[idx].w = result[idx - 1].w
result[idx].x = result[idx - 1].x
result[idx].y = result[idx - 1].y
result[idx].z = result[idx - 1].z
result[idx][index] = value
else:
result[idx] = result[idx - 1] + deltaScale * delta
return result
def read_quaternion(io_stream):
quat = Quaternion((0, 0, 0, 0))
quat.x = read_float(io_stream)
quat.y = read_float(io_stream)
quat.z = read_float(io_stream)
quat.w = read_float(io_stream)
return quat
def QuaternionLookRotation(fro, to, up): vector = (fro - to).normalized() vector2 = up.normalized().cross(vector).normalized() vector3 = vector.cross(vector2) m00 = vector2.x m01 = vector2.y m02 = vector2.z m10 = vector3.x m11 = vector3.y m12 = vector3.z m20 = vector.x m21 = vector.y m22 = vector.z num8 = (m00 + m11) + m22 q = Quaternion() if num8 > 0: num = sqrt(num8 + 1) q.w = num * 0.5 num = 0.5 / num q.x = (m12 - m21) * num q.y = (m20 - m02) * num q.z = (m01 - m10) * num elif m00 >= m11 and m00 >= m22: num7 = sqrt(((1 + m00) - m11) - m22) num4 = 0.5 / num7 q.x = 0.5 * num7 q.y = (m01 + m10) * num4 q.z = (m02 + m20) * num4 q.w = (m12 - m21) * num4 elif m11 > m22: num6 = sqrt(((1 + m11) - m00) - m22) num3 = 0.5 / num6 q.x = (m10+ m01) * num3 q.y = 0.5 * num6 q.z = (m21 + m12) * num3 q.w = (m20 - m02) * num3 else: num5 = sqrt(((1 + m22) - m00) - m11) num2 = 0.5 / num5 q.x = (m20 + m02) * num2 q.y = (m21 + m12) * num2 q.z = 0.5 * num5 q.w = (m01 - m10) * num2 return q
def QuaternionLookRotation(fro, to, up):
vector = (fro - to).normalized()
vector2 = up.normalized().cross(vector).normalized()
vector3 = vector.cross(vector2)
m00 = vector2.x
m01 = vector2.y
m02 = vector2.z
m10 = vector3.x
m11 = vector3.y
m12 = vector3.z
m20 = vector.x
m21 = vector.y
m22 = vector.z
num8 = (m00 + m11) + m22
q = Quaternion()
if num8 > 0:
num = sqrt(num8 + 1)
q.w = num * 0.5
num = 0.5 / num
q.x = (m12 - m21) * num
q.y = (m20 - m02) * num
q.z = (m01 - m10) * num
elif m00 >= m11 and m00 >= m22:
num7 = sqrt(((1 + m00) - m11) - m22)
num4 = 0.5 / num7
q.x = 0.5 * num7
q.y = (m01 + m10) * num4
q.z = (m02 + m20) * num4
q.w = (m12 - m21) * num4
elif m11 > m22:
num6 = sqrt(((1 + m11) - m00) - m22)
num3 = 0.5 / num6
q.x = (m10 + m01) * num3
q.y = 0.5 * num6
q.z = (m21 + m12) * num3
q.w = (m20 - m02) * num3
else:
num5 = sqrt(((1 + m22) - m00) - m11)
num2 = 0.5 / num5
q.x = (m20 + m02) * num2
q.y = (m21 + m12) * num2
q.z = 0.5 * num5
q.w = (m01 - m10) * num2
return q
def lerpq(value, bounds):
ret = Quaternion()
mult = Vector(bounds.mult)
add = Vector(bounds.add)
ret.x = add.x + value.x * mult.x
ret.y = add.y + value.y * mult.y
ret.z = add.z + value.z * mult.z
ret.w = add.w + value.w * mult.w
return ret
def qΔ由v一轴f二弧生成qLIB(v轴,f弧度):
q=Quaternion();
f半弧 = f弧度 *0.5;
fSin半弧 = sin(f半弧);
q.x = v轴.x * fSin半弧;
q.y = v轴.y * fSin半弧;
q.z = v轴.z * fSin半弧;
q.w = cos(f半弧);
return q;
def angle_between_nor(nor_orig, nor_result):
angle = math.acos(nor_orig.dot(nor_result))
axis = nor_orig.cross(nor_result).normalized()
q = Quaternion()
q.x = axis.x * math.sin(angle / 2)
q.y = axis.y * math.sin(angle / 2)
q.z = axis.z * math.sin(angle / 2)
q.w = math.cos(angle / 2)
return q
def angle_between_nor(nor_orig, nor_result):
angle = math.acos(nor_orig.dot(nor_result))
axis = nor_orig.cross(nor_result).normalized()
q = Quaternion()
q.x = axis.x * math.sin(angle / 2)
q.y = axis.y * math.sin(angle / 2)
q.z = axis.z * math.sin(angle / 2)
q.w = math.cos(angle / 2)
return q
def rotation_to(a, b):
"""Calculates shortest Quaternion from Vector a to Vector b"""
# a - up vector
# b - direction to point to
# http://stackoverflow.com/questions/1171849/finding-quaternion-representing-the-rotation-from-one-vector-to-another
# https://github.com/toji/gl-matrix/blob/f0583ef53e94bc7e78b78c8a24f09ed5e2f7a20c/src/gl-matrix/quat.js#L54
a = a.normalized()
b = b.normalized()
q = Quaternion()
tmpvec3 = Vector()
xUnitVec3 = Vector((1, 0, 0))
yUnitVec3 = Vector((0, 1, 0))
dot = a.dot(b)
if (dot < -0.999999):
# tmpvec3 = cross(xUnitVec3, a)
tmpvec3 = xUnitVec3.cross(a)
if (tmpvec3.length < 0.000001):
tmpvec3 = yUnitVec3.cross(a)
tmpvec3.normalize()
# q = Quaternion(tmpvec3, Math.PI)
q = Quaternion(tmpvec3, math.pi)
elif (dot > 0.999999):
q.x = 0
q.y = 0
q.z = 0
q.w = 1
else:
tmpvec3 = a.cross(b)
q.x = tmpvec3[0]
q.y = tmpvec3[1]
q.z = tmpvec3[2]
q.w = 1 + dot
q.normalize()
return q
def decode(data, channel, scale):
scaleFactor = float(1.0)
if data.bitCount == 8:
scaleFactor = 1 / float(16)
result = [None] * channel.numTimeCodes
result[0] = data.initialValue
for i, deltaBlock in enumerate(data.deltaBlocks):
blockScale = delta_table[deltaBlock.blockIndex]
deltaScale = blockScale * scale * scaleFactor
vectorIndex = deltaBlock.vectorIndex
deltas = get_deltas(deltaBlock, data.bitCount)
for j, delta in enumerate(deltas):
idx = int(i / channel.vectorLen) * 16 + j + 1
if idx >= channel.numTimeCodes:
break
if channel.type == 6:
# access quat as xyzw instead of wxyz
index = (vectorIndex + 1) % 4
value = result[idx - 1][index] + deltaScale * delta
if (result[idx] == None):
result[idx] = Quaternion()
result[idx].w = result[idx - 1].w
result[idx].x = result[idx - 1].x
result[idx].y = result[idx - 1].y
result[idx].z = result[idx - 1].z
result[idx][index] = value
else:
result[idx] = result[idx - 1] + deltaScale * delta
return result
def _arc_segment(v_1, v_2):
ELorigin = bpy.context.scene.objects['ELorigin']
ELground = bpy.context.scene.objects['ELground']
v = v_2 - v_1
d = v.length
ELorigin.location = Vector((0, 0, 0))
ELground.location = Vector((0, 0, -d))
v_L = ELground.location - ELorigin.location
q = Quaternion()
c = Vector.cross(v_L, v)
q.x = c.x
q.y = c.y
q.z = c.z
q.w = sqrt((v_L.length ** 2) * (v.length ** 2)) + \
Vector.dot(v_L, v)
q.normalize()
euler = q.to_euler()
bpy.ops.object.runfslg_operator()
laALL = bpy.context.scene.objects['laALL']
laALL.name = 'lARC'
laALL.rotation_euler = euler
laALL.location = v_1
bpy.context.active_object.select = False
laALL.select = True
bpy.ops.object.transform_apply(location=True, rotation=True, scale=True)
laALL.select = False
bpy.context.active_object.select = True
return laALL
def _arc_segment(v_1, v_2): ELorigin = bpy.context.scene.objects['ELorigin'] ELground = bpy.context.scene.objects['ELground'] v = v_2 - v_1 d = v.length ELorigin.location = Vector((0, 0, 0)) ELground.location = Vector((0, 0, -d)) v_L = ELground.location - ELorigin.location q = Quaternion() c = Vector.cross(v_L, v) q.x = c.x q.y = c.y q.z = c.z q.w = sqrt((v_L.length ** 2) * (v.length ** 2)) + \ Vector.dot(v_L, v) q.normalize() euler = q.to_euler() bpy.ops.object.runfslg_operator() laALL = bpy.context.scene.objects['laALL'] laALL.name = 'lARC' laALL.rotation_euler = euler laALL.location = v_1 bpy.context.active_object.select = False laALL.select = True bpy.ops.object.transform_apply(location=True, rotation=True, scale=True) laALL.select = False bpy.context.active_object.select = True return laALL
def orientacion(escena, objeto):
"""
Parameters
----------
escena : bpy.scene
Escena de nuestro entorno.
objeto : bpy.context.object
Objeto de nuestra escena.
Returns
-------
q : Quaternion
Cuaternion de rotación y lateral.
"""
e = bpy.context.object.AnimSettings.eje_inclinacion
e_l = bpy.context.object.AnimSettings.eje_lateral
if e == "ejeX+":
e = Vector((1, 0, 0))
elif e == "ejeX-":
e = Vector((-1, 0, 0))
elif e == "ejeY+":
e = Vector((0, 1, 0))
elif e == "ejeY-":
e = Vector((0, -1, 0))
elif e == "ejeZ+":
e = Vector((0, 0, 1))
elif e == "ejeZ-":
e = Vector((0, 0, -1))
if e_l == "ejeX+":
e_l = Vector((1, 0, 0))
elif e_l == "ejeX-":
e_l = Vector((-1, 0, 0))
elif e_l == "ejeY+":
e_l = Vector((0, 1, 0))
elif e_l == "ejeY-":
e_l = Vector((0, -1, 0))
elif e_l == "ejeZ+":
e_l = Vector((0, 0, 1))
elif e_l == "ejeZ-":
e_l = Vector((0, 0, -1))
# Alinear el objeto con la tangente
f_actual = escena.frame_current
f_anterior = f_actual - 1
pos_act = Vector(objeto.location)
escena.frame_set(f_anterior)
pos_ant = Vector(objeto.location)
t = (pos_act - pos_ant).normalized()
vec = Vector.cross(e, t).normalized()
c1 = clamp(e.dot(t), -1, 1)
theta = acos(c1)
s = cos(theta / 2.0)
w = sin(theta / 2.0) * vec
q1 = Quaternion()
q1.w = s
q1.x = w[0]
q1.y = w[1]
q1.z = w[2]
# Inclinar el coche lateralmente
e_l_rotate = Vector(e_l)
e_l_rotate.rotate(q1)
z = Vector((0, 0, 1))
l = t.cross(z)
l.normalize()
c2 = clamp(e_l_rotate.dot(l), -1, 1)
theta_l = acos(c2)
if e_l_rotate[2] < 0:
theta_l = -theta_l
s = cos(theta_l / 2.0)
w = sin(theta_l / 2.0) * t
q2 = Quaternion()
q2.w = s
q2.x = w[0]
q2.y = w[1]
q2.z = w[2]
if bpy.context.object.AnimSettings.inclinacion_bool == True:
inclinacion = bpy.context.object.AnimSettings.inclinacion
theta_i = math.pi * inclinacion / 180
s = cos(theta_i / 2.0)
w = sin(theta_i / 2.0) * e
q3 = Quaternion()
q3.w = s
q3.x = w[0]
q3.y = w[1]
q3.z = w[2]
q = q2 @ q1 @ q3
else:
q = q2 @ q1
return q
def read_bones(self, context, filepath, root):
skeleton_node = root.find("Skeleton")
if (skeleton_node == None):
return None, None
bones = []
bones_tag = []
flags_list = []
# LimitRotation and Unk0 have their special meanings, can be deduced if needed when exporting
flags_restricted = set(["LimitRotation", "Unk0"])
drawable_name = root.find("Name").text.split(".")[0]
bones_node = skeleton_node.find("Bones")
armature = context.object
bpy.ops.object.mode_set(mode='EDIT')
for bones_item in bones_node:
name_item = bones_item.find("Name")
tag_item = bones_item.find("Tag")
parentindex_item = bones_item.find("ParentIndex")
flags_item = bones_item.find("Flags")
translation_item = bones_item.find("Translation")
rotation_item = bones_item.find("Rotation")
scale_item = bones_item.find("Scale")
quaternion = Quaternion()
quaternion.w = float(rotation_item.attrib["w"])
quaternion.x = float(rotation_item.attrib["x"])
quaternion.y = float(rotation_item.attrib["y"])
quaternion.z = float(rotation_item.attrib["z"])
mat_rot = quaternion.to_matrix().to_4x4()
trans = Vector()
trans.x = float(translation_item.attrib["x"])
trans.y = float(translation_item.attrib["y"])
trans.z = float(translation_item.attrib["z"])
mat_loc = Matrix.Translation(trans)
scale = Vector()
scale.x = float(scale_item.attrib["x"])
scale.y = float(scale_item.attrib["y"])
scale.z = float(scale_item.attrib["z"])
mat_sca = Matrix.Scale(1, 4, scale)
edit_bone = armature.data.edit_bones.new(name_item.text)
# edit_bone.bone_id = int(bone_tag.attrib["value"])
if parentindex_item.attrib["value"] != "-1":
edit_bone.parent = armature.data.edit_bones[int(
parentindex_item.attrib["value"])]
# https://github.com/LendoK/Blender_GTA_V_model_importer/blob/master/importer.py
edit_bone.head = (0, 0, 0)
edit_bone.tail = (0, 0.05, 0)
edit_bone.matrix = mat_loc @ mat_rot @ mat_sca
if edit_bone.parent != None:
edit_bone.matrix = edit_bone.parent.matrix @ edit_bone.matrix
if (flags_item != None and flags_item.text != None):
flags = flags_item.text.strip().split(", ")
flags_list.append(flags)
# build a bones lookup table
bones.append(name_item.text)
bones_tag.append(int(tag_item.get('value')))
bpy.ops.object.mode_set(mode='POSE')
for i in range(len(bones)):
armature.pose.bones[i].bone.bone_properties.tag = bones_tag[i]
for _flag in flags_list[i]:
if (_flag in flags_restricted):
continue
flag = armature.pose.bones[i].bone.bone_properties.flags.add()
flag.name = _flag
bpy.ops.object.mode_set(mode='OBJECT')
return bones, drawable_name
def step(scene):
global dt
sub_steps = 1#*scene.jiggle.sub_steps)
dt = 1.0/(scene.render.fps*sub_steps)
for o in scene.objects:
if(o.type == 'ARMATURE'):
ow = o.matrix_world.copy()
scale = ow.col[0].length
children_of_bone = defaultdict(list)
for bone in o.pose.bones:
if bone.parent is not None:
children_of_bone[bone.parent].append(bone)
for _ in range( sub_steps):
bl = []
for b in o.pose.bones:
if(b.parent==None):
propB(ow,b,bl,None,children_of_bone)
bl2 = []
for wb in bl:
b = wb.b
# o------ -> ---o---
wb.restW = b.bone.matrix_local.copy() * scale
wb.Q = wb.Q.normalized()
if(b.bone.jiggle.enabled):
#(wb.parent.restW.inverted() * wb.restW) #
Jb = b.bone.jiggle
wb.rest = b.bone.matrix_local #
if(b.parent!=None):
wb.rest = b.bone.parent.matrix_local.inverted() @ wb.rest
wb.Kc = 0
if(Jb.control_bone!=""):
if(Jb.control_bone in o.pose.bones):
cb = o.pose.bones[Jb.control_bone]
clm = cb.matrix
if(cb.parent!=None):
clm = cb.parent.matrix.inverted() @ clm
wb.cQ = clm.to_quaternion().normalized()
wb.Kc = 1- pow(1-Jb.control, 1/scene.jiggle.iterations)
wb.rest_base = wb.rest.copy()
wb.rest.translation = wb.rest.translation * scale
wb.length = b.bone.length*scale
wb.irest = wb.rest.inverted()
wb.w = 1.0/Jb.mass
wb.k = 1- pow(1-Jb.Ks, 1/scene.jiggle.iterations)
Jb.V*= 1.0-Jb.Kld
Jb.V+= scene.gravity*dt
Jb.W*= 1.0-Jb.Kd
R = Jb.R.to_matrix()
wb.R = R.normalized()
wb.P = Jb.P.copy()
wb.Cx = wb.sample(0.5)
qv = Quaternion()
qv.x = Jb.W[0]
qv.y = Jb.W[1]
qv.z = Jb.W[2]
qv.w = 0
cv = wb.Cx + Jb.V*dt
wb.Q = qadd(wb.Q, [email protected]*dt*0.5).normalized() #newton's first law
wb.P += cv - wb.sample(0.5)#the same
bl2.append(wb)
for i in range(scene.jiggle.iterations):
for wb in bl2:
b = wb.b
if(b.parent==None):
continue
Jb = b.bone.jiggle
Pc = wb.P
target_m = [email protected]
Pt = target_m.translation.copy()
if(Jb.debug in scene.objects):
scene.objects[Jb.debug].location = Pc
W = wb.w + wb.parent.w
I = (Pc-Pt)/W
wb.applyImpulse(Pc,-I)
wb.parent.applyImpulse(Pt,I)
#for i in range(scene.jiggle.iterations):
for wb in bl2:
b = wb.b
if(b.parent==None):
continue
Jb = b.bone.jiggle
quatSpring(wb)
if(wb.Kc>0.0):
quatSpring(wb,wb.cQ, wb.Kc)
for wb in bl2:
b = wb.b
if(b.parent==None):
continue
Jb = b.bone.jiggle
wb.P = [email protected]
wb.R = wb.Q.normalized().to_matrix()
for wb in bl2:
b = wb.b
Jb = b.bone.jiggle
R = Jb.R.to_matrix()
Jb.V = (wb.sample(0.5) - wb.Cx)/dt
Jb.P = wb.P.copy()
wb.Q = wb.Q.normalized()
qv = [email protected]() #qadd(wb.Q,-Jb.R)*Jb.R.conjugated()#
Jb.W = Vector((qv.x,qv.y,qv.z))*(2/dt)
Jb.R = wb.Q
for wb in bl:
wb.R*=scale
for wb in bl2:
b = wb.b
pM = ow
if(b.parent!=None):
pM = wb.parent.M
b.matrix_basis = ([email protected]_base).inverted()@wb.M
scene.jiggle.last_frame+= 1
