WorldSim - 1.2.3

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  forceOnJoint = wVector(0.0, 0.0, 0.0);
31 
32  //--- calculate the localMatrixParent
33  localMatrixParent.x_ax = wVector::X();
34  localMatrixParent.y_ax = wVector::Y();
35  localMatrixParent.z_ax = wVector::Z();
36  localMatrixParent.w_pos = centre;
37  localMatrixParent.sanitifize();
38 
39  //--- calculate the local matrix respect to child object
40  if ( parent ) {
41  globalMatrixParent = localMatrixParent * parent->matrix();
42  } else {
43  globalMatrixParent = localMatrixParent;
44  }
45  localMatrixChild = globalMatrixParent * child->matrix().inverse();
46 
47  if ( !localMatrixParent.sanityCheck() ) {
48  qWarning( "Sanity Check Failed" );
49  }
50  if ( !localMatrixChild.sanityCheck() ) {
51  qWarning( "Sanity Check Failed" );
52  }
53 
54  if ( cp ) {
55  createPrivateJoint();
56  }
57 }
58 
59 wVector PhyBallAndSocket::centre() const {
60  //--- calculate global matrix if necessary
61  if ( parent() ) {
62  globalMatrixParent = localMatrixParent * parent()->matrix();
63  }
64  return globalMatrixParent.w_pos;
65 };
66 
67 wVector PhyBallAndSocket::getForceOnJoint() const
68 {
69  return forceOnJoint;
70 }
71 
72 void PhyBallAndSocket::createPrivateJoint() {
73 #ifdef WORLDSIM_USE_NEWTON
74  priv->joint = NewtonConstraintCreateUserJoint( worldpriv->world, 6, PhyJointPrivate::userBilateralHandler, 0, priv->child, priv->parent );
75  NewtonJointSetUserData( priv->joint, this );
76 #endif
77 }
78 
79 void PhyBallAndSocket::updateJointInfo() {
80  //--- calculate the global matrices
81  //--- if parent doesn't exist, then globalMatrixParent and localMatrixParent coincide and
82  //--- there is no need to re-assign it because it was already done in constructor
83  if ( parent() ) {
84  globalMatrixParent = localMatrixParent * parent()->matrix();
85  }
86  globalMatrixChild = localMatrixChild * child()->matrix();
87 
88  if ( !globalMatrixParent.sanityCheck() ) {
89  qWarning( "Sanity Check Failed" );
90  }
91  if ( !globalMatrixChild.sanityCheck() ) {
92  qWarning( "Sanity Check Failed" );
93  }
94 }
95 
96 void PhyBallAndSocket::updateJoint( real timestep ) {
97  //--- calculate the global matrices
98  //--- if parent doesn't exist, then globalMatrixParent and localMatrixParent coincide and
99  //--- there is no need to re-assign it because it was already done in constructor
100  if ( parent() ) {
101  globalMatrixParent = localMatrixParent * parent()->matrix();
102  }
103  globalMatrixChild = localMatrixChild * child()->matrix();
104 
105  if ( !globalMatrixParent.sanityCheck() ) {
106  qWarning( "Sanity Check Failed" );
107  }
108  if ( !globalMatrixChild.sanityCheck() ) {
109  qWarning( "Sanity Check Failed" );
110  }
111 
112 #ifdef WORLDSIM_USE_NEWTON
113  UNUSED_PARAM( timestep );
114  //--- Restrict the movement on the centre of the joint along all tree orthonormal direction
115  NewtonUserJointAddLinearRow( priv->joint, &globalMatrixChild.w_pos[0], &globalMatrixParent.w_pos[0], &globalMatrixParent.x_ax[0] );
116  NewtonUserJointSetRowStiffness( priv->joint, 1.0f );
117  NewtonUserJointAddLinearRow( priv->joint, &globalMatrixChild.w_pos[0], &globalMatrixParent.w_pos[0], &globalMatrixParent.y_ax[0] );
118  NewtonUserJointSetRowStiffness( priv->joint, 1.0f );
119  NewtonUserJointAddLinearRow( priv->joint, &globalMatrixChild.w_pos[0], &globalMatrixParent.w_pos[0], &globalMatrixParent.z_ax[0] );
120  NewtonUserJointSetRowStiffness( priv->joint, 1.0f );
121 
122  //--- Retrive forces applied to the joint by the constraints
123  forceOnJoint.x = NewtonUserJointGetRowForce (priv->joint, 0);
124  forceOnJoint.y = NewtonUserJointGetRowForce (priv->joint, 1);
125  forceOnJoint.z = NewtonUserJointGetRowForce (priv->joint, 2);
126 #endif
127 
128 }
129 
130 } // end namespace farsa