WorldSim - 1.2.0

Utilities Configuration GA
NNFW WorldSim Total 99% Experiments
phyballandsocket.cpp
1 /********************************************************************************
2  * WorldSim -- library for robot simulations *
3  * Copyright (C) 2008-2011 Gianluca Massera <emmegian@yahoo.it> *
4  * *
5  * This program is free software; you can redistribute it and/or modify *
6  * it under the terms of the GNU General Public License as published by *
7  * the Free Software Foundation; either version 2 of the License, or *
8  * (at your option) any later version. *
9  * *
10  * This program is distributed in the hope that it will be useful, *
11  * but WITHOUT ANY WARRANTY; without even the implied warranty of *
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
13  * GNU General Public License for more details. *
14  * *
15  * You should have received a copy of the GNU General Public License *
16  * along with this program; if not, write to the Free Software *
17  * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA *
18  ********************************************************************************/
19 
20 #include "phyballandsocket.h"
21 #include "private/phyjointprivate.h"
22 #include "private/phyobjectprivate.h"
23 #include "private/worldprivate.h"
24 
25 namespace farsa {
26 
27 PhyBallAndSocket::PhyBallAndSocket( const wVector& centre, PhyObject* parent, PhyObject* child, bool cp )
28  : PhyJoint( parent, child ) {
29  dofv = 0;
30  //--- calculate the localMatrixParent
31  localMatrixParent.x_ax = wVector::X();
32  localMatrixParent.y_ax = wVector::Y();
33  localMatrixParent.z_ax = wVector::Z();
34  localMatrixParent.w_pos = centre;
35  localMatrixParent.sanitifize();
36 
37  //--- calculate the local matrix respect to child object
38  if ( parent ) {
39  globalMatrixParent = localMatrixParent * parent->matrix();
40  } else {
41  globalMatrixParent = localMatrixParent;
42  }
43  localMatrixChild = globalMatrixParent * child->matrix().inverse();
44 
45  if ( !localMatrixParent.sanityCheck() ) {
46  qWarning( "Sanity Check Failed" );
47  }
48  if ( !localMatrixChild.sanityCheck() ) {
49  qWarning( "Sanity Check Failed" );
50  }
51 
52  if ( cp ) {
53  createPrivateJoint();
54  }
55 }
56 
57 wVector PhyBallAndSocket::centre() const {
58  //--- calculate global matrix if necessary
59  if ( parent() ) {
60  globalMatrixParent = localMatrixParent * parent()->matrix();
61  }
62  return globalMatrixParent.w_pos;
63 };
64 
65 void PhyBallAndSocket::createPrivateJoint() {
66 #ifdef WORLDSIM_USE_NEWTON
67  priv->joint = NewtonConstraintCreateUserJoint( worldpriv->world, 6, PhyJointPrivate::userBilateralHandler, 0, priv->child, priv->parent );
68  NewtonJointSetUserData( priv->joint, this );
69 #endif
70 }
71 
72 void PhyBallAndSocket::updateJointInfo() {
73  //--- calculate the global matrices
74  //--- if parent doesn't exist, then globalMatrixParent and localMatrixParent coincide and
75  //--- there is no need to re-assign it because it was already done in constructor
76  if ( parent() ) {
77  globalMatrixParent = localMatrixParent * parent()->matrix();
78  }
79  globalMatrixChild = localMatrixChild * child()->matrix();
80 
81  if ( !globalMatrixParent.sanityCheck() ) {
82  qWarning( "Sanity Check Failed" );
83  }
84  if ( !globalMatrixChild.sanityCheck() ) {
85  qWarning( "Sanity Check Failed" );
86  }
87 }
88 
89 void PhyBallAndSocket::updateJoint( real timestep ) {
90  //--- calculate the global matrices
91  //--- if parent doesn't exist, then globalMatrixParent and localMatrixParent coincide and
92  //--- there is no need to re-assign it because it was already done in constructor
93  if ( parent() ) {
94  globalMatrixParent = localMatrixParent * parent()->matrix();
95  }
96  globalMatrixChild = localMatrixChild * child()->matrix();
97 
98  if ( !globalMatrixParent.sanityCheck() ) {
99  qWarning( "Sanity Check Failed" );
100  }
101  if ( !globalMatrixChild.sanityCheck() ) {
102  qWarning( "Sanity Check Failed" );
103  }
104 
105 #ifdef WORLDSIM_USE_NEWTON
106  UNUSED_PARAM( timestep );
107  //--- Restrict the movement on the centre of the joint along all tree orthonormal direction
108  NewtonUserJointAddLinearRow( priv->joint, &globalMatrixChild.w_pos[0], &globalMatrixParent.w_pos[0], &globalMatrixParent.x_ax[0] );
109  NewtonUserJointSetRowStiffness( priv->joint, 1.0f );
110  NewtonUserJointAddLinearRow( priv->joint, &globalMatrixChild.w_pos[0], &globalMatrixParent.w_pos[0], &globalMatrixParent.y_ax[0] );
111  NewtonUserJointSetRowStiffness( priv->joint, 1.0f );
112  NewtonUserJointAddLinearRow( priv->joint, &globalMatrixChild.w_pos[0], &globalMatrixParent.w_pos[0], &globalMatrixParent.z_ax[0] );
113  NewtonUserJointSetRowStiffness( priv->joint, 1.0f );
114 
115 #endif
116 
117 }
118 
119 } // end namespace farsa