# setRotationの考察
# よけいなコード 変数があるが後から利用するためおいておく
# 重力なし 衝突なし ただvpytonとbodyを描いただけ
# C:\myprg_main\python_my_prg\ode_prg\setRotation.py
import ode
from visual import *
from math import *
boxList = []
ballList = []
cylList = []
box_kosuu=5
ball_kosuu=5
def torad(d):
return d/180.0*math.pi
#http://www.not-enough.org/abe/manual/api-aa09/linearConv1.html
#3X3の行列と、3次ベクトルとの積を計算する関数
def mulmatrix(m, v):
vv = [0, 0, 0]
for i in range(3):
vv[i] = m[i][0]*v[0] + m[i][1]*v[1] + m[i][2]*v[2]
return vv
#ベクトルvをX軸中心にd度だけ回転させる関数
def rotateX(v, d):
r = torad(d)
s = math.sin(r)
c = math.cos(r)
m = [[1, 0, 0],
[0, c, -s],
[0, s, c]]
#X軸中心に回転させる行列を作成
return mulmatrix(m, v)
#行列とベクトルの積を計算
def rotateY(v, d):
r = torad(d)
s = math.sin(r)
c = math.cos(r)
m = [[c, 0, s],
[0, 1, 0],
[-s, 0, c]]
return mulmatrix(m, v)
def rotateZ(v, d):
r = torad(d)
s = math.sin(r)
c = math.cos(r)
m = [[c, -s, 0],
[s, c, 0],
[0, 0, 1]]
return mulmatrix(m, v)
def kaiten_gyouretu2(x, y, z):
rx = float(x)
ry = float(y)
rz = float(z)
vx= [1.0, 0.0, 0.0]
vx= rotateX(vx, rx)
vx= rotateY(vx, ry)
vx= rotateZ(vx, rz)
vy= [0.0, 1.0, 0.0]
vy= rotateX(vy, rx)
vy= rotateY(vy, ry)
vy= rotateZ(vy, rz)
vz= [0.0, 0.0, 1.0]
vz= rotateX(vz, rx)
vz= rotateY(vz, ry)
vz= rotateZ(vz, rz)
print "vvvvvvvvvvvvvv"
print vx
print vy
print vz
#●オブジェクトの回転からodeのオブジェクトを回転させる回転行列を求める
#回転行列：rot_list
#def kaiten_gyouretu(kakudo_x, kakudo_y, kakudo_z):
# radx = kakudo_x/360*2*math.pi
# cox = math.cos(radx)
# six = math.sin(radx)
# #lx = [1.0,0.0,0.0, 0.0,cox,-six, 0.0,six,cox]
# lx = [1.0,0.0,0.0, 0.0,cox,six, 0.0,-six,cox]
# #x軸の回転
# rady = kakudo_y/360*2*math.pi
# coy = math.cos(rady)
# siy = math.sin(rady)
# #ly = [coy,0.0,siy, 0.0,1.0,0.0, -siy,0.0,coy]
# ly = [coy,0.0,-siy, 0.0,1.0,0.0, siy,0.0,coy]
# #y軸の回転
#
# radz = kakudo_z/360*2*math.pi
# coz = math.cos(radz)
# siz = math.sin(radz)
# #lz = [coz,-siz,0.0, siz,coz,0.0, 0.0,0.0,1.0]
# lz = [coz,siz,0.0, -siz,coz,0.0, 0.0,0.0,1.0]
# #ｚ軸の回転
#
# #rot_list = gyouretu_seki(gyouretu_seki(lx,ly), lz)
# rot_list = gyouretu_seki(gyouretu_seki(lz,ly), lx)
# print "rot_list0000000000000"
# print rot_list[0],
# print rot_list[1],
# print rot_list[2]
# print rot_list[3],
# print rot_list[4],
# print rot_list[5]
# print rot_list[6],
# print rot_list[7],
# print rot_list[8]
# return rot_list
#各回転をくみあわせるには、行列の積の計算が必要
def gyouretu_seki(la,lb):
seki_list = [
la[0]*lb[0] + la[1]*lb[3] + la[2]*lb[6], la[0]*lb[1] + la[1]*lb[4] + la[2]*lb[7], la[0]*lb[2] + la[1]*lb[5] + la[2]*lb[8],
la[3]*lb[0] + la[4]*lb[3] + la[5]*lb[6], la[3]*lb[1] + la[4]*lb[4] + la[5]*lb[7], la[3]*lb[2] + la[4]*lb[5] + la[5]*lb[8],
la[6]*lb[0] + la[7]*lb[3] + la[8]*lb[6], la[6]*lb[1] + la[7]*lb[4] + la[8]*lb[7], la[6]*lb[2] + la[7]*lb[5] + la[8]*lb[8]
]
print "seki_list"
print seki_list[0],
print seki_list[1],
print seki_list[2]
print seki_list[3],
print seki_list[4],
print seki_list[5]
print seki_list[6],
print seki_list[7],
print seki_list[8]
return seki_list
#任意の単位ベクトル(nx,ny,nz)まわりにd回転
def rotater( nx, ny, nz, d):
r = torad(d)
s = math.sin(r)
c = math.cos(r)
m = [nx**2*(1-c)+c, nx*ny*(1-c)-nz*s, nz*nx*(1-c)+ny*s,
nx*ny*(1-c)+nz*s, ny**2*(1-c)+c, ny*nz*(1-c)-nx*s,
nz*nx*(1-c)-ny*s, ny*nz*(1-c)+nx*s, nz**2*(1-c)+c]
#行列とベクトルの積を計算
return m
#seki_list
#0.866025403784 -0.5 0.0
#0.5 0.866025403784 0.0
#0.0 0.0 1.0
#self.geom.setRotation([1,0,0, 0,0.866,-0.5, 0,0.5,0.866]) #x軸30°回転
#0.866025403784 -0.433012701892 -0.25
#0.25 0.808012701892 -0.533493649054
#0.433012701892 0.399519052838 0.808012701892
def metaRotater(x, y, z):
lx = rotater(1.0, 0.0, 0.0, x) #x軸でx度回転
y_new = rotateX([0.0, 1.0, 0.0], -x) #y軸が回転しまったので元のy軸のベクトルを求める
z_new = rotateX([0.0, 0.0, 1.0], -x) #z軸が回転しまったので元のz軸のベクトルを求める
print "y_new"
print y_new
ly = rotater(y_new[0], y_new[1], y_new[2], y) #元のy軸でy回転
x_new = rotateY([1.0, 0.0, 0.0], -y) #x軸が回転しまったので元のx軸のベクトルを求める
z_new = rotateY(z_new, -y) #z軸が回転しまったので元のz軸のベクトルを求める
lz = rotater(z_new[0], z_new[1], z_new[2], z) #元のz軸でz回転
rot_list = gyouretu_seki(gyouretu_seki(lx,ly), lz)
return rot_list
class Field:
scene = display(autoscale=0, forward=norm((-2.0,-1.0,-1.0)))
#物理世界を作成
world=ode.World()
#world.setGravity((0, -9.81, 0))
world.setGravity((0, 0, 0))
#地平面を作成？
space=ode.Space()
ode_floor=ode.GeomPlane(space,(0,1,0),0)
ode_floor.setCategoryBits(1)
ode_floor.setCollideBits(3)
ode_floor.viz=box(
pos=(0,-0.03,0),
width=20,length=20,height=0.06,color=(0.5,0.5,1.0))
#衝突関係
jointgroup=ode.JointGroup()
vball_x=sphere( pos=(0, 0, 0), radius=0.2, color=color.cyan) #原点
vball_x=sphere( pos=(5, 0, 0), radius=0.2, color=color.red) #x軸
vball_y=sphere( pos=(0, 5, 0), radius=0.2, color=color.white)#y軸
vball_z=sphere( pos=(0, 0, 5), radius=0.2, color=color.yellow) #z軸
target_fps=30
#target_fps=30だから1秒間に30回の処理をする
dt=1.0/target_fps
def near_callback(self,args,geom1,geom2):
#isBall = ((ground == o1) || (ground == o2));
#if (isGround)//地面だけと衝突判定
print geom1
for c in ode.collide(geom1,geom2):
c.setBounce(1.0)
j=ode.ContactJoint(self.world,self.jointgroup,c)
j.attach(geom1.getBody(),geom2.getBody())
def tick(self):
for i in range(ball_kosuu):
ballList[i].update()
boxList[0].update()
#cylList[0].update()
time.sleep(0.5)
self.space.collide((),self.near_callback)
self.world.step(self.dt)
self.jointgroup.empty()
return True
class Box:
def __init__(
self, field,
m_density=0.01,
v_x=7, v_y=1, v_z=3,
b_pos=vector(0,3,0),
v_color=color.cyan
):
#デフォルトで...(vpthonのパラメータだけですます)
self.m_x = v_x
self.m_y = v_y
self.m_z = v_z
self.g_x = v_x
self.g_y = v_y
self.g_z = v_z
self.body=ode.Body(field.world)
M=ode.Mass()
M.setBox(m_density, self.m_z, self.m_y, self.m_x)#密度,z,y,x w,h,l
self.body.setMass(M)
self.body.setPosition(b_pos)
self.geom = ode.GeomBox(
space=field.space, lengths=(self.g_z, self.g_y, self.g_x)#w,h,l
)
self.geom.setCategoryBits(1)
self.geom.setCollideBits(7)
#ボール同士の衝突を避ける
self.geom.setBody(self.body)
#self.geom.setRotation([0.866,0,0.5, 0,1,0, -0.5,0,0.866])#y軸30°回転
#self.geom.setRotation([1,0,0, 0,0.866,-0.5, 0,0.5,0.866]) #x軸30°回転
#self.geom.setRotation([0.866,-0.5,0, 0.5,0.866,0, 0,0,1]) #z軸30°回転
#self.geom.setRotation(kaiten_gyouretu(30.0, 0.0, 30.0)) #z軸30°回転
#kaiten_gyouretu(30.0, 0.0, 0.0)
#metaRotater(30.0, 0.0, 0.0)
self.geom.setRotation(metaRotater(30.0, 0.0, 30.0)) #z軸30°回転
mat=self.geom.getRotation()
self.f = frame()
self.f.pos = b_pos
self.f.axis=(mat[0],mat[3],mat[6])
self.f.up=mat[1],mat[4],mat[7]
self.vbox = box(frame=self.f,
length=v_x, height=v_y, width=v_z, color=v_color)
#mat=self.geom.getRotation()
#self.f.axis=(10,-10,0)
#self.f.axis=(mat[0],mat[3],mat[6])
#self.f.up=mat[1],mat[4],mat[7]
#self.f.axis=(1, 1, 1)
#self.vbox.up=mat[1],mat[4],mat[7]
#setRotationとmatの表示が微妙にずれるのでうえのコードで
#vpytonのオブジェクトをmatで表示してからまたvpythonの
#回転こまんどで微調整をしよう
#self.geom.setRotation([0.866,0,0.5, 0,1,0, -0.5,0,0.866])
# opacity ：透明度
#BoxのMassの大きさ,geomの大きさ,
# vboxの大きさを同じにする
#print m_x,m_y,m_z
def update(self):
print 'box_update'
pos = self.geom.getPosition()
#boxのジオメトリの位置を得る
#self.vbox.pos = pos
self.f.pos = pos
mat=self.geom.getRotation()
#self.f.axis=(mat[0],mat[3],mat[6])
#self.f.up=mat[1],mat[4],mat[7]
if __name__=='__main__':
field = Field()
box = Box(field)
#lx = rotater(0, 0, 1, 30.0) #x軸でx度回転
#y_new = rotateX([0, 1, 0], -30.0) #y軸が回転しまったので元のy軸のベクトルを求める
#print y_new[0]
# #while True:
# while Flg:
# field.tick()
0 件のコメント:
コメントを投稿