mbdyn.interface.vectors

1 #!/usr/bin/env python 2 # -*- coding: utf-8 -*- 3 # 4 # This file is part of MBDyn sim suite. 5 # Copyright (C) 2007 André ESPAZE, as part of a Master thesis supervised by 6 # Martin O.L.Hansen (DTU) and Nicolas Chauvat (Logilab) 7 8 # MBDyn sim suite is free software; you can redistribute it and/or modify 9 # it under the terms of the GNU General Public License as published by 10 # the Free Software Foundation; either version 2 of the License, or 11 # (at your option) any later version. 12 # 13 # This program is distributed in the hope that it will be useful, 14 # but WITHOUT ANY WARRANTY; without even the implied warranty of 15 # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 # GNU General Public License for more details. 17 # 18 # You should have received a copy of the GNU General Public License 19 # along with this program; if not, write to the Free Software 20 # Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. 21 # 22 """VTK reference frames and vectors for the MBDyn post processing. 23 Until now, the main class used is L{ReferenceFrame} for representing 24 the orientation matrix of the nodes and reference frames. The L{Vector} 25 class could also been used for representing velocities as a future 26 development and it will certainly be a based to represent 27 angular velocities as well. 28 """ 29 import vtk 30 import numpy as N 31 32

33 -def get_norm(array):

34 """Return the norm of a 3x1 array""" 35 res = N.sqrt(N.dot(array.transpose(), array)) 36 return res[0][0]

37

38 -def fill_rotation_matrix(rmat, angle, axis):

39 """Fill the C{rmat} rotation matrix from an angle 40 and axis of rotation""" 41 qaxis = N.sin(angle/2) * axis 42 w = N.cos(angle/2) 43 x = qaxis[0] 44 y = qaxis[1] 45 z = qaxis[2] 46 47 xx2 = 2*x*x 48 yy2 = 2*y*y 49 zz2 = 2*z*z 50 xy2 = 2*x*y 51 wz2 = 2*w*z 52 zx2 = 2*z*x 53 wy2 = 2*w*y 54 yz2 = 2*y*z 55 wx2 = 2*w*x 56 57 rmat[0, 0] = 1. - yy2 - zz2 58 rmat[0, 1] = xy2 - wz2 59 rmat[0, 2] = zx2 + wy2 60 rmat[1, 0] = xy2 + wz2 61 rmat[1, 1] = 1. - xx2 - zz2 62 rmat[1, 2] = yz2 - wx2 63 rmat[2, 0] = zx2 - wy2 64 rmat[2, 1] = yz2 + wx2 65 rmat[2, 2] = 1. - xx2 - yy2 66 return rmat

67 68

69 -class VTKCommon:

70 """The common class for a VTK object in MBDyn. It should 71 be moved to the L{mbdyn.interface.common} module. 72 """ 73

74 - def __init__(self, name):

75 self.name = name 76 self.scale_factor = 1.

77

78 - def set_scale_factor(self, scale_factor):

79 """Set a scale factor on the object""" 80 self.scale_factor = scale_factor

81 82

83 -class Arrow(VTKCommon):

84 """Description of an arrow mapper. 85 """ 86

87 - def __init__(self, name="vector"):

88 VTKCommon.__init__(self, name) 89 self.radius = 1. 90 self.radius_ratio = 2. 91 self.length_ratio = 0.2 92 93 self.initial_orientation = N.array([ [0.], 94 [1.], 95 [0.] ]) 96 self.initial_orientation_t = self.initial_orientation.transpose() 97 self.initial_orientation_r = self.initial_orientation.reshape(3) 98 self.source = { 99 "cone" : vtk.vtkConeSource(), 100 "cyl" : vtk.vtkCylinderSource() 101 } 102 self.transfo = {} 103 polydata = vtk.vtkAppendPolyData() 104 for key in ["cone", "cyl"]: 105 self.transfo[key] = vtk.vtkTransform() 106 vfilter = vtk.vtkTransformPolyDataFilter() 107 vfilter.SetInput(self.source[key].GetOutput()) 108 vfilter.SetTransform(self.transfo[key]) 109 polydata.AddInput(vfilter.GetOutput()) 110 111 # To debug 112 #sp = vtk.vtkSphereSource() 113 #sp.SetRadius(1.5) 114 #polydata.AddInput(sp.GetOutput()) 115 116 self.mapper = vtk.vtkPolyDataMapper() 117 self.mapper.SetInput(polydata.GetOutput()) 118 119 self.set_scale_factor(1.) 120 self.set_norm(30.) 121 self.set_resolution(20)

122

123 - def set_scale_factor(self, scale_factor):

124 """Update the arrow radius""" 125 self.scale_factor = scale_factor 126 self.set_radius(self.scale_factor)

127

128 - def set_radius_radio(self, radius_ratio):

129 """Set the radius ratio between the cone 130 and the cylinder""" 131 self.radius_ratio = radius_ratio 132 self.set_radius(self.radius)

133

134 - def set_radius(self, radius):

135 """Set the arrow radius""" 136 self.radius = radius 137 self.source["cyl"].SetRadius(radius) 138 self.source["cone"].SetRadius(radius * self.radius_ratio)

139

140 - def set_resolution(self, resolution):

141 """Set the cylinder and cone resolution""" 142 for key in ["cyl", "cone"]: 143 self.source[key].SetResolution(resolution)

144

145 - def set_norm(self, norm):

146 """Set the norm of the arrow""" 147 height = norm 148 cone_height = height * self.length_ratio 149 cyl_height = height - cone_height 150 self.source["cyl"].SetHeight(cyl_height) 151 self.source["cone"].SetHeight(cone_height) 152 153 self.transfo["cyl"].Identity() 154 # The base of the cylinder need to be the point of reference 155 # By default, this is the middle point in VTK 156 self.transfo["cyl"].Translate(0., cyl_height/2., 0.) 157 158 self.transfo["cone"].Identity() 159 # Cone will be aligned with the cylinder 160 self.transfo["cone"].RotateZ(90.) 161 # Same remark as the cylinder but the cone has been rotated 162 # of 90 degrees around the z axis and needs to stay 163 # at the top of the cylinder. 164 self.transfo["cone"].Translate(cyl_height + cone_height/2., 0., 0.)

165

166 - def get_rotation_matrix(self, current_orientation):

167 """The rotation matrix is got according to the 168 initial orientation of the C{polydata}. 169 """ 170 rotation_matrix = N.eye(3) 171 angle = N.arccos(N.dot(self.initial_orientation_t, 172 current_orientation)[0][0]) 173 if angle == N.pi: 174 rotation_matrix[1, 1] = -1 175 else: 176 axis = N.cross(self.initial_orientation_r, 177 current_orientation.reshape(3)) 178 norm = get_norm(axis.reshape(3, 1)) 179 if norm != 0.: 180 axis = axis/norm 181 fill_rotation_matrix(rotation_matrix, angle, axis) 182 return rotation_matrix

183 184

185 -class Vector:

186 """Description of a vector as a VTK actor""" 187

188 - def __init__(self):

189 self.actor = vtk.vtkActor() 190 self.transfo_mat = vtk.vtkMatrix4x4() 191 self.transfo_mat.SetElement(3, 3, 1.) 192 self.actor.SetUserMatrix(self.transfo_mat) 193 194 self.text = vtk.vtkVectorText() 195 self.text.SetText("Origin") 196 text_mapper = vtk.vtkPolyDataMapper() 197 text_mapper.SetInputConnection(self.text.GetOutputPort()) 198 self.text_actor = vtk.vtkFollower() 199 self.text_actor.SetMapper(text_mapper) 200 #textActor.SetScale(0.2, 0.2, 0.2) 201 #textActor.AddPosition(0, -0.1, 0) 202 203 self.arrow = None 204 self.position = N.zeros((3, 1)) 205 self.rotation_matrix = N.zeros((3, 3)) 206 self.value = N.zeros((3, 1)) 207 208 self.set_arrow(Arrow()) 209 #self.define(N.zeros((3,1)), N.eye(3)) 210 self.set_value(N.array([ [0.], 211 [30.], 212 [0.] ]))

213 214

215 - def set_arrow(self, arrow):

216 """Set the arrow mapper for the vector""" 217 self.arrow = arrow 218 self.actor.SetMapper(arrow.mapper)

219

220 - def set_position(self, pos_array):

221 """Set the position of the vector""" 222 self.define(pos_array, self.rotation_matrix)

223

224 - def set_rotation_matrix(self, rotation_matrix):

225 """Set the rotation (or orientation) matrix 226 of the vector""" 227 self.define(self.position, rotation_matrix)

228

229 - def define(self, position_array, rotation_matrix):

230 """A vector is defined by an position and a rotation 231 matrix in the VTK world""" 232 self.position = position_array 233 self.rotation_matrix = rotation_matrix 234 for jdx in range(3): 235 for idx in range(3): 236 self.transfo_mat.SetElement(idx, jdx, 237 rotation_matrix[idx, jdx]) 238 for idx in range(3): 239 self.transfo_mat.SetElement(idx, 3, 240 position_array[idx][0])

241 #print self.transfo_mat 242

243 - def set_value(self, array):

244 """Set the vector value""" 245 self.value = array.copy() 246 array_norm = get_norm(array) 247 normalized_array = array.copy() / array_norm 248 self.arrow.set_norm(array_norm) 249 self.set_rotation_matrix_from(normalized_array)

250

251 - def set_rotation_matrix_from(self, normalized_array):

252 """Set the rotation matrix of a vector from a normalized array 253 that represent only its current rotation.""" 254 rotmatrix = self.arrow.get_rotation_matrix(normalized_array) 255 self.set_rotation_matrix(rotmatrix)

256

257 - def set_color(self, rval, gval, bval):

258 """Set the color by using RGB values coming from 259 the U{GIMP<http://www.gimp.org/>}.""" 260 self.actor.GetProperty().SetColor(rval/255., 261 gval/255., 262 bval/255.)

263 264

265 -class CommonAxe(Vector):

266 """The top class of every axe. 267 """ 268

269 - def __init__(self, direction):

270 Vector.__init__(self) 271 self.direction = direction 272 self.set_value(N.asarray(30. * self.direction))

273

274 - def display_from(self, orientation_matrix):

275 """Display the axe according to its orientation matrix""" 276 current_orientation = N.asarray(orientation_matrix * \ 277 self.direction) 278 self.set_rotation_matrix_from(current_orientation)

279 280

281 -class XAxe(CommonAxe):

282 """The M{x} axe, set as red. 283 """ 284

285 - def __init__(self):

286 direction = N.matrix([ [1.], 287 [0.], 288 [0.] ]) 289 CommonAxe.__init__(self, direction) 290 self.set_color(243., 17., 17.)

291 292

293 -class YAxe(CommonAxe):

294 """The M{y} axe, set as green. 295 """ 296

297 - def __init__(self):

298 direction = N.matrix([ [0.], 299 [1.], 300 [0.] ]) 301 CommonAxe.__init__(self, direction) 302 self.set_color(34., 193., 13.)

303 304

305 -class ZAxe(CommonAxe):

306 """The M{z} axe, set as blue. 307 """ 308

309 - def __init__(self):

310 direction = N.matrix([ [0.], 311 [0.], 312 [1.] ]) 313 CommonAxe.__init__(self, direction) 314 self.set_color(21., 115., 232.)

315 316 317

318 -class ReferenceFrame:

319 """A VTK reference frame able to represent 320 a MBDyn rotation (or orientation matrix). 321 """ 322

323 - def __init__(self):

324 self.axes = [] 325 for axe_class in [XAxe, YAxe, ZAxe]: 326 self.axes.append(axe_class()) 327 self.set_position(N.zeros((3, 1))) 328 329 self.orientation_matrix = None 330 self.set_orientation_matrix(N.eye(3))

331

332 - def set_orientation_matrix(self, orientation_matrix):

333 """Display the reference frame from the MBDyn orientation 334 matrix""" 335 self.orientation_matrix = orientation_matrix 336 for axe in self.axes: 337 axe.display_from(orientation_matrix)

338

339 - def set_scale_factor(self, scale_factor):

340 """Scale the reference frame""" 341 for axe in self.axes: 342 axe.arrow.set_scale_factor(scale_factor)

343

344 - def set_axes_norm(self, norm):

345 """Set the norm for each vector axe""" 346 for axe in self.axes: 347 axe.arrow.set_norm(norm)

348

349 - def add_into(self, vtk_renderer):

350 """Add the reference frame into the VTK area (or renderer)""" 351 for axe in self.axes: 352 vtk_renderer.AddActor(axe.actor)

353

354 - def remove_from(self, vtk_renderer):

355 """Remove the reference frame from the VTK area (or renderer)""" 356 for axe in self.axes: 357 vtk_renderer.RemoveActor(axe.actor)

358

359 - def set_position(self, array):

360 """Set the position of the reference frame""" 361 for axe in self.axes: 362 axe.set_position(array)

363

364 - def set_arrow(self, arrow):

365 """Set the arrow mapper for every axe""" 366 for axe in self.axes: 367 axe.set_arrow(arrow)

368

Source Code for Module mbdyn.interface.vectors