WorldSim - 1.2.3

Utilities Configuration GA
NNFW WorldSim Total 99% Experiments
phyfixed.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 "phyfixed.h"
21 #include "private/phyjointprivate.h"
22 #include "private/phyobjectprivate.h"
23 #include "private/worldprivate.h"
24 
25 namespace farsa {
26 
27 PhyFixed::PhyFixed( 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 = wVector(0, 0, 0);
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 PhyFixed::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 PhyFixed::getForceOnJoint() const
68 {
69  return forceOnJoint;
70 }
71 
72 void PhyFixed::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 PhyFixed::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 PhyFixed::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  //--- In order to constraint the rotation about X and Y axis of the joint
123  //--- we use LinearRow (that are stronger) getting a point far from objects along
124  //--- the Z axis. Doing this if the two object rotates about X or Y axes then
125  //--- the difference between qChild and qParent augments and then Newton Engine will apply
126  //--- a corresponding force (that applyied outside the centre of the object will become
127  //--- a torque) that will blocks any rotation about X and Y axis
128  real len = 5000.0;
129  wVector qChild( globalMatrixChild.w_pos + globalMatrixChild.z_ax.scale(len) );
130  wVector qParent( globalMatrixParent.w_pos + globalMatrixParent.z_ax.scale(len) );
131  NewtonUserJointAddLinearRow( priv->joint, &qChild[0], &qParent[0], &globalMatrixParent.x_ax[0] );
132  NewtonUserJointSetRowStiffness( priv->joint, 1.0 );
133  NewtonUserJointAddLinearRow( priv->joint, &qChild[0], &qParent[0], &globalMatrixParent.y_ax[0] );
134  NewtonUserJointSetRowStiffness( priv->joint, 1.0 );
135 
136  NewtonUserJointAddAngularRow( priv->joint, 0.0f, &globalMatrixParent.z_ax[0] );
137  NewtonUserJointSetRowStiffness( priv->joint, 1.0f );
138 
139  //--- In order to do the same with third axis (Z), I need others point along different axis
140 /* qChild = wVector( globalMatrixChild.w_pos + globalMatrixChild.y_ax.scale(len) );
141  qParent = wVector( globalMatrixParent.w_pos + globalMatrixParent.y_ax.scale(len) );
142  NewtonUserJointAddLinearRow( priv->joint, &qChild[0], &qParent[0], &globalMatrixParent.z_ax[0] );
143  NewtonUserJointSetRowStiffness( priv->joint, 1.0 );*/
144 
145  //--- Retrive forces applied to the joint by the constraints
146  forceOnJoint.x = NewtonUserJointGetRowForce (priv->joint, 0);
147  forceOnJoint.y = NewtonUserJointGetRowForce (priv->joint, 1);
148  forceOnJoint.z = NewtonUserJointGetRowForce (priv->joint, 2);
149 
150 #endif
151 
152 }
153 
154 } // end namespace farsa