1 /* This file is part of StepCore library.
2 Copyright (C) 2007 Vladimir Kuznetsov <ks.vladimir@gmail.com>
3
4 StepCore library is free software; you can redistribute it and/or modify
5 it under the terms of the GNU General Public License as published by
6 the Free Software Foundation; either version 2 of the License, or
7 (at your option) any later version.
8
9 StepCore library is distributed in the hope that it will be useful,
10 but WITHOUT ANY WARRANTY; without even the implied warranty of
11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 GNU General Public License for more details.
13
14 You should have received a copy of the GNU General Public License
15 along with StepCore; if not, write to the Free Software
16 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
17 */
18
19 #include "rigidbody.h"
20 #include "types.h"
21 #include <cstring>
22 #include <cmath>
23
24 namespace StepCore
25 {
26
27 STEPCORE_META_OBJECT(RigidBody, QT_TRANSLATE_NOOP("ObjectClass", "RigidBody"), QT_TRANSLATE_NOOP("ObjectDescription", "Generic rigid body"), 0, STEPCORE_SUPER_CLASS(Item) STEPCORE_SUPER_CLASS(Body),
28 STEPCORE_PROPERTY_RW_D(StepCore::Vector2d, position, QT_TRANSLATE_NOOP("PropertyName", "position"), QT_TRANSLATE_NOOP("Units", "m"), QT_TRANSLATE_NOOP("PropertyDescription", "Position of the center of mass"), position, setPosition)
29 STEPCORE_PROPERTY_RW_D(double, angle, QT_TRANSLATE_NOOP("PropertyName", "angle"), QT_TRANSLATE_NOOP("Units", "rad"), QT_TRANSLATE_NOOP("PropertyDescription", "Rotation angle"), angle, setAngle)
30
31 STEPCORE_PROPERTY_RW_D(StepCore::Vector2d, velocity, QT_TRANSLATE_NOOP("PropertyName", "velocity"), QT_TRANSLATE_NOOP("Units", "m/s"), QT_TRANSLATE_NOOP("PropertyDescription", "Velocity of the center of mass"), velocity, setVelocity)
32 STEPCORE_PROPERTY_RW_D(double, angularVelocity, QT_TRANSLATE_NOOP("PropertyName", "angularVelocity"), QT_TRANSLATE_NOOP("Units", "rad/s"), QT_TRANSLATE_NOOP("PropertyDescription", "Angular velocity of the body"), angularVelocity, setAngularVelocity)
33
34 STEPCORE_PROPERTY_R_D(StepCore::Vector2d, acceleration, QT_TRANSLATE_NOOP("PropertyName", "acceleration"), STEPCORE_FROM_UTF8(QT_TRANSLATE_NOOP("Units", "m/s²")),
35 QT_TRANSLATE_NOOP("PropertyDescription", "Acceleration of the center of mass"), acceleration)
36 STEPCORE_PROPERTY_R_D(double, angularAcceleration, QT_TRANSLATE_NOOP("PropertyName", "angularAcceleration"), STEPCORE_FROM_UTF8(QT_TRANSLATE_NOOP("Units", "rad/s²")),
37 QT_TRANSLATE_NOOP("PropertyDescription", "Angular acceleration of the body"), angularAcceleration)
38
39 STEPCORE_PROPERTY_R_D(StepCore::Vector2d, force, QT_TRANSLATE_NOOP("PropertyName", "force"), QT_TRANSLATE_NOOP("Units", "N"), QT_TRANSLATE_NOOP("PropertyDescription", "Force that acts upon the body"), force)
40 STEPCORE_PROPERTY_R_D(double, torque, QT_TRANSLATE_NOOP("PropertyName", "torque"), QT_TRANSLATE_NOOP("Units", "N m"), QT_TRANSLATE_NOOP("PropertyDescription", "Torque that acts upon the body"), torque)
41
42 STEPCORE_PROPERTY_RW(double, mass, QT_TRANSLATE_NOOP("PropertyName", "mass"), QT_TRANSLATE_NOOP("Units", "kg"), QT_TRANSLATE_NOOP("PropertyDescription", "Total mass of the body"), mass, setMass)
43 STEPCORE_PROPERTY_RW(double, inertia, QT_TRANSLATE_NOOP("PropertyName", "inertia"), STEPCORE_FROM_UTF8(QT_TRANSLATE_NOOP("Units", "kg m²")),
44 QT_TRANSLATE_NOOP("PropertyDescription", "Inertia \"tensor\" of the body"), inertia, setInertia)
45 STEPCORE_PROPERTY_RWF(StepCore::Vector2d, momentum, QT_TRANSLATE_NOOP("PropertyName", "momentum"), QT_TRANSLATE_NOOP("Units", "kg m/s"), QT_TRANSLATE_NOOP("PropertyDescription", "momentum"),
46 StepCore::MetaProperty::DYNAMIC, momentum, setMomentum)
47 STEPCORE_PROPERTY_RWF(double, angularMomentum, QT_TRANSLATE_NOOP("PropertyName", "angularMomentum"), STEPCORE_FROM_UTF8(QT_TRANSLATE_NOOP("Units", "kg m² rad/s")), QT_TRANSLATE_NOOP("PropertyDescription", "angular momentum"),
48 StepCore::MetaProperty::DYNAMIC, angularMomentum, setAngularMomentum)
49 STEPCORE_PROPERTY_RWF(double, kineticEnergy, QT_TRANSLATE_NOOP("PropertyName", "kineticEnergy"), QT_TRANSLATE_NOOP("Units", "J"), QT_TRANSLATE_NOOP("PropertyDescription", "kinetic energy"),
50 StepCore::MetaProperty::DYNAMIC, kineticEnergy, setKineticEnergy))
51
52 STEPCORE_META_OBJECT(RigidBodyErrors, QT_TRANSLATE_NOOP("ObjectClass", "RigidBodyErrors"), QT_TRANSLATE_NOOP("ObjectDescription", "Errors class for RigidBody"), 0, STEPCORE_SUPER_CLASS(ObjectErrors),
53 STEPCORE_PROPERTY_RW_D(StepCore::Vector2d, positionVariance, QT_TRANSLATE_NOOP("PropertyName", "positionVariance"), QT_TRANSLATE_NOOP("Units", "m"),
54 QT_TRANSLATE_NOOP("PropertyDescription", "position variance"), positionVariance, setPositionVariance)
55 STEPCORE_PROPERTY_RW_D(double, angleVariance, QT_TRANSLATE_NOOP("PropertyName", "angleVariance"), QT_TRANSLATE_NOOP("Units", "rad"),
56 QT_TRANSLATE_NOOP("PropertyDescription", "angle variance"), angleVariance, setAngleVariance)
57
58 STEPCORE_PROPERTY_RW_D(StepCore::Vector2d, velocityVariance, QT_TRANSLATE_NOOP("PropertyName", "velocityVariance"), QT_TRANSLATE_NOOP("Units", "m/s"),
59 QT_TRANSLATE_NOOP("PropertyDescription", "velocity variance"), velocityVariance, setVelocityVariance)
60 STEPCORE_PROPERTY_RW_D(double, angularVelocityVariance, QT_TRANSLATE_NOOP("PropertyName", "angularVelocityVariance"), QT_TRANSLATE_NOOP("Units", "rad/s"),
61 QT_TRANSLATE_NOOP("PropertyDescription", "angularVelocity variance"), angularVelocityVariance, setAngularVelocityVariance)
62
63 STEPCORE_PROPERTY_R_D(StepCore::Vector2d, accelerationVariance, QT_TRANSLATE_NOOP("PropertyName", "accelerationVariance"), STEPCORE_FROM_UTF8(QT_TRANSLATE_NOOP("Units", "m/s²")),
64 QT_TRANSLATE_NOOP("PropertyDescription", "acceleration variance"), accelerationVariance)
65 STEPCORE_PROPERTY_R_D(double, angularAccelerationVariance, QT_TRANSLATE_NOOP("PropertyName", "angularAccelerationVariance"), STEPCORE_FROM_UTF8(QT_TRANSLATE_NOOP("Units", "rad/s²")),
66 QT_TRANSLATE_NOOP("PropertyDescription", "angularAcceleration variance"), angularAccelerationVariance)
67
68 STEPCORE_PROPERTY_R_D(StepCore::Vector2d, forceVariance, QT_TRANSLATE_NOOP("PropertyName", "forceVariance"), QT_TRANSLATE_NOOP("Units", "N"), QT_TRANSLATE_NOOP("PropertyDescription", "force variance"), forceVariance)
69 STEPCORE_PROPERTY_R_D(double, torqueVariance, QT_TRANSLATE_NOOP("PropertyName", "torqueVariance"), QT_TRANSLATE_NOOP("Units", "N m"), QT_TRANSLATE_NOOP("PropertyDescription", "torque variance"), torqueVariance)
70
71 STEPCORE_PROPERTY_RW(double, massVariance, QT_TRANSLATE_NOOP("PropertyName", "massVariance"), QT_TRANSLATE_NOOP("Units", "kg"),
72 QT_TRANSLATE_NOOP("PropertyDescription", "mass variance"), massVariance, setMassVariance )
73 STEPCORE_PROPERTY_RW(double, inertiaVariance, QT_TRANSLATE_NOOP("PropertyName", "inertiaVariance"), STEPCORE_FROM_UTF8(QT_TRANSLATE_NOOP("Units", "kg m²")),
74 QT_TRANSLATE_NOOP("PropertyDescription", "inertia variance"), inertiaVariance, setInertiaVariance )
75 STEPCORE_PROPERTY_RWF(StepCore::Vector2d, momentumVariance, QT_TRANSLATE_NOOP("PropertyName", "momentumVariance"), QT_TRANSLATE_NOOP("Units", "kg m/s"),
76 QT_TRANSLATE_NOOP("PropertyDescription", "momentum variance"), StepCore::MetaProperty::DYNAMIC, momentumVariance, setMomentumVariance)
77 STEPCORE_PROPERTY_RWF(double, angularMomentumVariance, QT_TRANSLATE_NOOP("PropertyName", "angularMomentumVariance"), STEPCORE_FROM_UTF8(QT_TRANSLATE_NOOP("Units", "kg m² rad/s")),
78 QT_TRANSLATE_NOOP("PropertyDescription", "angular momentum variance"), StepCore::MetaProperty::DYNAMIC,
79 angularMomentumVariance, setAngularMomentumVariance)
80 STEPCORE_PROPERTY_RWF(double, kineticEnergyVariance, QT_TRANSLATE_NOOP("PropertyName", "kineticEnergyVariance"), QT_TRANSLATE_NOOP("Units", "J"),
81 QT_TRANSLATE_NOOP("PropertyDescription", "kinetic energy variance"), StepCore::MetaProperty::DYNAMIC, kineticEnergyVariance, setKineticEnergyVariance))
82
83 STEPCORE_META_OBJECT(Disk, QT_TRANSLATE_NOOP("ObjectClass", "Disk"), QT_TRANSLATE_NOOP("ObjectDescription", "Rigid disk"), 0, STEPCORE_SUPER_CLASS(RigidBody),
84 STEPCORE_PROPERTY_RW(double, radius, QT_TRANSLATE_NOOP("PropertyName", "radius"), QT_TRANSLATE_NOOP("Units", "m"), QT_TRANSLATE_NOOP("PropertyDescription", "Radius of the disk"), radius, setRadius))
85
86 STEPCORE_META_OBJECT(BasePolygon, QT_TRANSLATE_NOOP("ObjectClass", "BasePolygon"), QT_TRANSLATE_NOOP("ObjectDescription", "Base polygon body"), 0, STEPCORE_SUPER_CLASS(RigidBody),)
87
88 STEPCORE_META_OBJECT(Box, QT_TRANSLATE_NOOP("ObjectClass", "Box"), QT_TRANSLATE_NOOP("ObjectDescription", "Rigid box"), 0, STEPCORE_SUPER_CLASS(BasePolygon),
89 STEPCORE_PROPERTY_RW(StepCore::Vector2d, size, QT_TRANSLATE_NOOP("PropertyName", "size"), QT_TRANSLATE_NOOP("Units", "m"), QT_TRANSLATE_NOOP("PropertyDescription", "Size of the box"), size, setSize))
90
91 STEPCORE_META_OBJECT(Polygon, QT_TRANSLATE_NOOP("ObjectClass", "Polygon"), QT_TRANSLATE_NOOP("ObjectDescription", "Rigid polygon body"), 0, STEPCORE_SUPER_CLASS(BasePolygon),
92 STEPCORE_PROPERTY_RW(Vector2dList, vertexes, QT_TRANSLATE_NOOP("PropertyName", "vertices"), QT_TRANSLATE_NOOP("Units", "m"), QT_TRANSLATE_NOOP("PropertyDescription", "Vertex list"), vertexes, setVertexes))
93
rigidBody() const94 RigidBody* RigidBodyErrors::rigidBody() const
95 {
96 return static_cast<RigidBody*>(owner());
97 }
98
accelerationVariance() const99 Vector2d RigidBodyErrors::accelerationVariance() const
100 {
101 return _forceVariance/square(rigidBody()->mass()) +
102 _massVariance*(rigidBody()->force()/square(rigidBody()->mass())).array().square().matrix();
103 }
104
angularAccelerationVariance() const105 double RigidBodyErrors::angularAccelerationVariance() const
106 {
107 return _torqueVariance/square(rigidBody()->inertia()) +
108 _inertiaVariance*square(rigidBody()->torque()/square(rigidBody()->inertia()));
109 }
110
momentumVariance() const111 Vector2d RigidBodyErrors::momentumVariance() const
112 {
113 return _velocityVariance * square(rigidBody()->mass()) +
114 rigidBody()->velocity().array().square().matrix() * _massVariance;
115 }
116
setMomentumVariance(const Vector2d & momentumVariance)117 void RigidBodyErrors::setMomentumVariance(const Vector2d& momentumVariance)
118 {
119 _velocityVariance = (momentumVariance - rigidBody()->velocity().array().square().matrix() * _massVariance) /
120 square(rigidBody()->mass());
121 }
122
angularMomentumVariance() const123 double RigidBodyErrors::angularMomentumVariance() const
124 {
125 return _angularVelocityVariance * square(rigidBody()->inertia()) +
126 square(rigidBody()->angularVelocity()) * _inertiaVariance;
127 }
128
setAngularMomentumVariance(double angularMomentumVariance)129 void RigidBodyErrors::setAngularMomentumVariance(double angularMomentumVariance)
130 {
131 _angularVelocityVariance =
132 (angularMomentumVariance - square(rigidBody()->angularVelocity()) * _inertiaVariance) /
133 square(rigidBody()->inertia());
134 }
135
kineticEnergyVariance() const136 double RigidBodyErrors::kineticEnergyVariance() const
137 {
138 return (rigidBody()->velocity().array().square().matrix()).dot(_velocityVariance) * square(rigidBody()->mass()) +
139 square(rigidBody()->velocity().squaredNorm()/2) * _massVariance +
140 _angularVelocityVariance * square(rigidBody()->angularVelocity() * rigidBody()->inertia()) +
141 square(square(rigidBody()->angularVelocity())/2) * _inertiaVariance;
142 }
143
setKineticEnergyVariance(double kineticEnergyVariance)144 void RigidBodyErrors::setKineticEnergyVariance(double kineticEnergyVariance)
145 {
146 double t = kineticEnergyVariance - this->kineticEnergyVariance() +
147 (rigidBody()->velocity().array().square().matrix()).dot(_velocityVariance) * square(rigidBody()->mass());
148 _velocityVariance = t / square(rigidBody()->mass()) / 2 *
149 (rigidBody()->velocity().array().square().inverse().matrix());
150 if(!std::isfinite(_velocityVariance[0]) || _velocityVariance[0] < 0 ||
151 !std::isfinite(_velocityVariance[1]) || _velocityVariance[1]) {
152 _velocityVariance.setZero();
153 }
154 // XXX: change angularVelocity here as well
155 }
156
RigidBody(const Vector2d & position,double angle,const Vector2d & velocity,double angularVelocity,double mass,double inertia)157 RigidBody::RigidBody(const Vector2d &position, double angle,
158 const Vector2d &velocity, double angularVelocity, double mass, double inertia)
159 : _position(position), _angle(angle), _velocity(velocity), _angularVelocity(angularVelocity),
160 _force(Vector2d::Zero()), _torque(0), _mass(mass), _inertia(inertia)
161 {
162 }
163
applyForce(const Vector2d & force,const Vector2d & position)164 void RigidBody::applyForce(const Vector2d& force, const Vector2d& position)
165 {
166 _force += force;
167 _torque += (position[0] - _position[0])*force[1] -
168 (position[1] - _position[1])*force[0]; // XXX: sign ?
169 }
170
applyForceVariance(const Vector2d & force,const Vector2d & position,const Vector2d & forceVariance,const Vector2d & positionVariance)171 void RigidBodyErrors::applyForceVariance(const Vector2d& force,
172 const Vector2d& position,
173 const Vector2d& forceVariance,
174 const Vector2d& positionVariance)
175 {
176 _forceVariance += forceVariance;
177 _torqueVariance += forceVariance[1] * square(position[0] - rigidBody()->_position[0]) +
178 forceVariance[0] * square(position[1] - rigidBody()->_position[1]) +
179 (positionVariance[0] + _positionVariance[0]) * square(force[1]) +
180 (positionVariance[1] + _positionVariance[1]) * square(force[0]);
181 }
182
velocityWorld(const Vector2d & worldPoint) const183 Vector2d RigidBody::velocityWorld(const Vector2d& worldPoint) const
184 {
185 Vector2d p = (worldPoint - _position)*_angularVelocity;
186 return _velocity + Vector2d(-p[1], p[0]);
187 }
188
velocityLocal(const Vector2d & localPoint) const189 Vector2d RigidBody::velocityLocal(const Vector2d& localPoint) const
190 {
191 Vector2d p = vectorLocalToWorld(localPoint)*_angularVelocity;
192 return _velocity + Vector2d(-p[1], p[0]);
193 }
194
pointLocalToWorld(const Vector2d & p) const195 Vector2d RigidBody::pointLocalToWorld(const Vector2d& p) const
196 {
197 double c = cos(_angle);
198 double s = sin(_angle);
199 return Vector2d( p[0]*c - p[1]*s + _position[0],
200 p[0]*s + p[1]*c + _position[1]);
201 }
202
pointWorldToLocal(const Vector2d & p) const203 Vector2d RigidBody::pointWorldToLocal(const Vector2d& p) const
204 {
205 double c = cos(_angle);
206 double s = sin(_angle);
207 return Vector2d( (p[0]-_position[0])*c + (p[1]-_position[1])*s,
208 -(p[0]-_position[0])*s + (p[1]-_position[1])*c);
209 }
210
vectorLocalToWorld(const Vector2d & v) const211 Vector2d RigidBody::vectorLocalToWorld(const Vector2d& v) const
212 {
213 double c = cos(_angle);
214 double s = sin(_angle);
215 return Vector2d( v[0]*c - v[1]*s,
216 v[0]*s + v[1]*c);
217 }
218
vectorWorldToLocal(const Vector2d & v) const219 Vector2d RigidBody::vectorWorldToLocal(const Vector2d& v) const
220 {
221 double c = cos(_angle);
222 double s = sin(_angle);
223 return Vector2d( v[0]*c + v[1]*s,
224 -v[0]*s + v[1]*c);
225 }
226
getVariables(double * position,double * velocity,double * positionVariance,double * velocityVariance)227 void RigidBody::getVariables(double* position, double* velocity,
228 double* positionVariance, double* velocityVariance)
229 {
230 Vector2d::Map(position) = _position;
231 Vector2d::Map(velocity) = _velocity;
232 position[2] = _angle;
233 velocity[2] = _angularVelocity;
234
235 if(positionVariance) {
236 RigidBodyErrors* re = rigidBodyErrors();
237 Vector2d::Map(positionVariance) = re->_positionVariance;
238 Vector2d::Map(velocityVariance) = re->_velocityVariance;
239 positionVariance[2] = re->_angleVariance;
240 velocityVariance[2] = re->_angularVelocityVariance;
241 }
242 }
243
setVariables(const double * position,const double * velocity,const double * positionVariance,const double * velocityVariance)244 void RigidBody::setVariables(const double* position, const double* velocity,
245 const double* positionVariance, const double* velocityVariance)
246 {
247 _position = Vector2d::Map(position);
248 _velocity = Vector2d::Map(velocity);
249 _angle = position[2];
250 _angularVelocity = velocity[2];
251
252 _force.setZero();
253 _torque = 0;
254
255 if(positionVariance) {
256 RigidBodyErrors* re = rigidBodyErrors();
257 re->_positionVariance = Vector2d::Map(positionVariance);
258 re->_velocityVariance = Vector2d::Map(velocityVariance);
259 re->_angleVariance = positionVariance[2];
260 re->_angularVelocityVariance = velocityVariance[2];
261
262 re->_forceVariance.setZero();
263 re->_torqueVariance = 0;
264 }
265 }
266
getAccelerations(double * acceleration,double * accelerationVariance)267 void RigidBody::getAccelerations(double* acceleration, double* accelerationVariance)
268 {
269 acceleration[0] = _force[0] / _mass;
270 acceleration[1] = _force[1] / _mass;
271 acceleration[2] = _torque / _inertia;
272 if(accelerationVariance) {
273 RigidBodyErrors* re = rigidBodyErrors();
274 accelerationVariance[0] = re->_forceVariance[0]/square(_mass) +
275 square(_force[0]/square(_mass))*re->_massVariance;
276 accelerationVariance[1] = re->_forceVariance[1]/square(_mass) +
277 square(_force[1]/square(_mass))*re->_massVariance;
278 accelerationVariance[2] = re->_torqueVariance/square(_inertia) +
279 square(_torque/square(_inertia))*re->_inertiaVariance;
280 }
281 }
282
addForce(const double * force,const double * forceVariance)283 void RigidBody::addForce(const double* force, const double* forceVariance)
284 {
285 _force[0] += force[0];
286 _force[1] += force[1];
287 _torque += force[2];
288 if(forceVariance) {
289 RigidBodyErrors* re = rigidBodyErrors();
290 re->_forceVariance[0] += forceVariance[0];
291 re->_forceVariance[1] += forceVariance[1];
292 re->_torqueVariance += forceVariance[2];
293 }
294 }
295
resetForce(bool resetVariance)296 void RigidBody::resetForce(bool resetVariance)
297 {
298 _force.setZero();
299 _torque = 0;
300 if(resetVariance) {
301 RigidBodyErrors* re = rigidBodyErrors();
302 re->_forceVariance.setZero();
303 re->_torqueVariance = 0;
304 }
305 }
306
getInverseMass(VectorXd * inverseMass,DynSparseRowMatrix * variance,int offset)307 void RigidBody::getInverseMass(VectorXd* inverseMass,
308 DynSparseRowMatrix* variance, int offset)
309 {
310 inverseMass->coeffRef(offset) = (1/_mass);
311 inverseMass->coeffRef(offset+1) = (1/_mass);
312 inverseMass->coeffRef(offset+2) = (1/_inertia);
313 if(variance) {
314 RigidBodyErrors* re = rigidBodyErrors();
315 double vm = re->_massVariance / square(square(_mass));
316 double vi = re->_inertiaVariance / square(square(_inertia));
317 variance->coeffRef(offset, offset) = ( vm);
318 variance->coeffRef(offset+1, offset+1) = ( vm);
319 variance->coeffRef(offset+2, offset+2) = ( vi);
320 }
321 }
322
setKineticEnergy(double kineticEnergy)323 void RigidBody::setKineticEnergy(double kineticEnergy)
324 {
325 double e = kineticEnergy - _inertia * square(_angularVelocity)/2;
326 if(e > 0) {
327 double v = _velocity.norm();
328 _velocity = sqrt(e*2/_mass) * (v>0 ? (_velocity/v).eval() : Vector2d(1,0));
329 } else {
330 _velocity.setZero();
331 _angularVelocity = sqrt(kineticEnergy*2/_inertia);
332 }
333 }
334
Box(const Vector2d & position,double angle,const Vector2d & velocity,double angularVelocity,double mass,double inertia,const Vector2d & size)335 Box::Box(const Vector2d &position, double angle,
336 const Vector2d &velocity, double angularVelocity,
337 double mass, double inertia, const Vector2d &size)
338 : BasePolygon(position, angle, velocity, angularVelocity, mass, inertia)
339 {
340 _vertexes.resize(4);
341 setSize(size);
342 }
343
setSize(const Vector2d & size)344 void Box::setSize(const Vector2d& size)
345 {
346 Vector2d s(size.array().abs().matrix()/2.0);
347
348 _vertexes[0] << -s[0], -s[1];
349 _vertexes[1] << s[0], -s[1];
350 _vertexes[2] << s[0], s[1];
351 _vertexes[3] << -s[0], s[1];
352 }
353
354 } // namespace StepCore
355
356