00001 /****************************************************************************** 00002 * SOFA, Simulation Open-Framework Architecture, version 1.0 beta 3 * 00003 * (c) 2006-2008 MGH, INRIA, USTL, UJF, CNRS * 00004 * * 00005 * This library is free software; you can redistribute it and/or modify it * 00006 * under the terms of the GNU Lesser General Public License as published by * 00007 * the Free Software Foundation; either version 2.1 of the License, or (at * 00008 * your option) any later version. * 00009 * * 00010 * This library is distributed in the hope that it will be useful, but WITHOUT * 00011 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * 00012 * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * 00013 * for more details. * 00014 * * 00015 * You should have received a copy of the GNU Lesser General Public License * 00016 * along with this library; if not, write to the Free Software Foundation, * 00017 * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. * 00018 ******************************************************************************* 00019 * SOFA :: Modules * 00020 * * 00021 * Authors: The SOFA Team and external contributors (see Authors.txt) * 00022 * * 00023 * Contact information: contact@sofa-framework.org * 00024 ******************************************************************************/ 00025 #include <sofa/component/constraint/ParabolicConstraint.inl> 00026 #include <sofa/core/ObjectFactory.h> 00027 #include <sofa/defaulttype/Vec3Types.h> 00028 #include <sofa/defaulttype/RigidTypes.h> 00029 //#include <sofa/helper/gl/Axis.h> 00030 00031 00032 namespace sofa 00033 { 00034 00035 namespace component 00036 { 00037 00038 namespace constraint 00039 { 00040 00041 using namespace sofa::defaulttype; 00042 using namespace sofa::helper; 00043 00044 00045 #ifndef SOFA_FLOAT 00046 template <> 00047 void ParabolicConstraint<Rigid3dTypes>::projectPosition(VecCoord& x) 00048 { 00049 Real t = (Real) getContext()->getTime(); 00050 00051 if ( t >= m_tBegin.getValue() && t <= m_tEnd.getValue() ) 00052 { 00053 Real relativeTime = (t - m_tBegin.getValue() ) / (m_tEnd.getValue() - m_tBegin.getValue()); 00054 const SetIndexArray & indices = m_indices.getValue().getArray(); 00055 00056 for(SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) 00057 { 00058 Real px = m_locP3.x()*relativeTime; 00059 Real py = (- m_locP2.y() / (m_locP3.x()*m_locP2.x() - m_locP2.x()*m_locP2.x())) * (px *px) + ( (m_locP3.x()*m_locP2.y()) / (m_locP3.x()*m_locP2.x() - m_locP2.x()*m_locP2.x())) * px; 00060 Vec3R locPos( px , py, 0.0); 00061 00062 Vec3R worldPos = m_P1.getValue() + m_projection.rotate(locPos); 00063 00064 x[*it].getCenter() = worldPos; 00065 } 00066 } 00067 } 00068 00069 template <> 00070 void ParabolicConstraint<Rigid3dTypes>::projectVelocity(VecDeriv& dx) 00071 { 00072 Real t = (Real) getContext()->getTime(); 00073 Real dt = (Real) getContext()->getDt(); 00074 00075 if ( t >= m_tBegin.getValue() && t <= m_tEnd.getValue() ) 00076 { 00077 Real relativeTime = (t - m_tBegin.getValue() ) / (m_tEnd.getValue() - m_tBegin.getValue()); 00078 const SetIndexArray & indices = m_indices.getValue().getArray(); 00079 00080 for(SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) 00081 { 00082 Real pxP = m_locP3.x()*relativeTime; 00083 Real pyP = (- m_locP2.y() / (m_locP3.x()*m_locP2.x() - m_locP2.x()*m_locP2.x())) * (pxP *pxP) + ( (m_locP3.x()*m_locP2.y()) / (m_locP3.x()*m_locP2.x() - m_locP2.x()*m_locP2.x())) * pxP; 00084 relativeTime = (t+dt - m_tBegin.getValue() ) / (m_tEnd.getValue() - m_tBegin.getValue()); 00085 Real pxN = m_locP3.x()*relativeTime; 00086 Real pyN = (- m_locP2.y() / (m_locP3.x()*m_locP2.x() - m_locP2.x()*m_locP2.x())) * (pxN *pxN) + ( (m_locP3.x()*m_locP2.y()) / (m_locP3.x()*m_locP2.x() - m_locP2.x()*m_locP2.x())) * pxN; 00087 00088 Vec3R locVel = Vec3R( (pxN-pxP)/dt, (pyN-pyP)/dt, 0.0); 00089 00090 Vec3R worldVel = m_projection.rotate(locVel); 00091 00092 dx[*it].getVCenter() = worldVel; 00093 } 00094 } 00095 } 00096 #endif 00097 #ifndef SOFA_DOUBLE 00098 template <> 00099 void ParabolicConstraint<Rigid3fTypes>::projectPosition(VecCoord& x) 00100 { 00101 Real t = (Real) getContext()->getTime(); 00102 00103 if ( t >= m_tBegin.getValue() && t <= m_tEnd.getValue() ) 00104 { 00105 Real relativeTime = (t - m_tBegin.getValue() ) / (m_tEnd.getValue() - m_tBegin.getValue()); 00106 const SetIndexArray & indices = m_indices.getValue().getArray(); 00107 00108 for(SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) 00109 { 00110 Real px = m_locP3.x()*relativeTime; 00111 Real py = (- m_locP2.y() / (m_locP3.x()*m_locP2.x() - m_locP2.x()*m_locP2.x())) * (px *px) + ( (m_locP3.x()*m_locP2.y()) / (m_locP3.x()*m_locP2.x() - m_locP2.x()*m_locP2.x())) * px; 00112 Vec3R locPos( px , py, 0.0); 00113 00114 Vec3R worldPos = m_P1.getValue() + m_projection.rotate(locPos); 00115 00116 x[*it].getCenter() = worldPos; 00117 } 00118 } 00119 } 00120 template <> 00121 void ParabolicConstraint<Rigid3fTypes>::projectVelocity(VecDeriv& dx) 00122 { 00123 Real t = (Real) getContext()->getTime(); 00124 Real dt = (Real) getContext()->getDt(); 00125 00126 if ( t >= m_tBegin.getValue() && t <= m_tEnd.getValue() ) 00127 { 00128 Real relativeTime = (t - m_tBegin.getValue() ) / (m_tEnd.getValue() - m_tBegin.getValue()); 00129 const SetIndexArray & indices = m_indices.getValue().getArray(); 00130 00131 for(SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) 00132 { 00133 Real pxP = m_locP3.x()*relativeTime; 00134 Real pyP = (- m_locP2.y() / (m_locP3.x()*m_locP2.x() - m_locP2.x()*m_locP2.x())) * (pxP *pxP) + ( (m_locP3.x()*m_locP2.y()) / (m_locP3.x()*m_locP2.x() - m_locP2.x()*m_locP2.x())) * pxP; 00135 relativeTime = (t+dt - m_tBegin.getValue() ) / (m_tEnd.getValue() - m_tBegin.getValue()); 00136 Real pxN = m_locP3.x()*relativeTime; 00137 Real pyN = (- m_locP2.y() / (m_locP3.x()*m_locP2.x() - m_locP2.x()*m_locP2.x())) * (pxN *pxN) + ( (m_locP3.x()*m_locP2.y()) / (m_locP3.x()*m_locP2.x() - m_locP2.x()*m_locP2.x())) * pxN; 00138 00139 Vec3R locVel = Vec3R( (pxN-pxP)/dt, (pyN-pyP)/dt, 0.0); 00140 00141 Vec3R worldVel = m_projection.rotate(locVel); 00142 00143 dx[*it].getVCenter() = worldVel; 00144 } 00145 } 00146 } 00147 #endif 00148 00149 00150 00151 00152 00153 00154 SOFA_DECL_CLASS(ParabolicConstraint) 00155 00156 00157 int ParabolicConstraintClass = core::RegisterObject("Apply a parabolic trajectory to given points") 00158 #ifndef SOFA_FLOAT 00159 .add< ParabolicConstraint<Vec3dTypes> >() 00160 .add< ParabolicConstraint<Rigid3dTypes> >() 00161 #endif 00162 #ifndef SOFA_DOUBLE 00163 .add< ParabolicConstraint<Vec3fTypes> >() 00164 .add< ParabolicConstraint<Rigid3fTypes> >() 00165 #endif 00166 ; 00167 00168 #ifndef SOFA_FLOAT 00169 template class ParabolicConstraint<Rigid3dTypes>; 00170 template class ParabolicConstraint<Vec3dTypes>; 00171 #endif 00172 #ifndef SOFA_DOUBLE 00173 template class ParabolicConstraint<Rigid3fTypes>; 00174 template class ParabolicConstraint<Vec3fTypes>; 00175 #endif 00176 00177 00178 00179 } // namespace constraint 00180 00181 } // namespace component 00182 00183 } // namespace sofa 00184
1.5.7.1