1 #ifndef SimTK_SIMBODY_CONSTRAINT_SPHERE_ON_SPHERE_CONTACT_IMPL_H_
2 #define SimTK_SIMBODY_CONSTRAINT_SPHERE_ON_SPHERE_CONTACT_IMPL_H_
3
4 /* -------------------------------------------------------------------------- *
5 * Simbody(tm) *
6 * -------------------------------------------------------------------------- *
7 * This is part of the SimTK biosimulation toolkit originating from *
8 * Simbios, the NIH National Center for Physics-Based Simulation of *
9 * Biological Structures at Stanford, funded under the NIH Roadmap for *
10 * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. *
11 * *
12 * Portions copyright (c) 2014 Stanford University and the Authors. *
13 * Authors: Michael Sherman *
14 * Contributors: *
15 * *
16 * Licensed under the Apache License, Version 2.0 (the "License"); you may *
17 * not use this file except in compliance with the License. You may obtain a *
18 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. *
19 * *
20 * Unless required by applicable law or agreed to in writing, software *
21 * distributed under the License is distributed on an "AS IS" BASIS, *
22 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
23 * See the License for the specific language governing permissions and *
24 * limitations under the License. *
25 * -------------------------------------------------------------------------- */
26
27 /**@file
28 Private implementation of Constraint::SphereOnSphereContact. **/
29
30 #include "SimTKmath.h"
31 #include "simbody/internal/common.h"
32 #include "simbody/internal/Constraint.h"
33
34 #include "ConstraintImpl.h"
35
36 class SimbodyMatterSubsystemRep;
37
38 namespace SimTK {
39
40 class SimbodyMatterSubsystem;
41 class SimbodyMatterSubtree;
42 class MobilizedBody;
43
44 //==============================================================================
45 // SPHERE ON SPHERE CONTACT IMPL
46 //==============================================================================
47 class Constraint::SphereOnSphereContactImpl : public ConstraintImpl {
48 public:
49
50 // An object of this local type is used as the value of a Position-stage
51 // discrete state variable that allows sphere parameters to be changed for a
52 // particular State.
53 struct Parameters {
ParametersParameters54 Parameters(const Vec3& p_FSf, Real rad_F, const Vec3& p_BSb, Real rad_B)
55 : m_p_FSf(p_FSf), m_radius_F(rad_F), m_p_BSb(p_BSb), m_radius_B(rad_B) {}
56
57 Parameters() = default;
58
59 Vec3 m_p_FSf{NaN}; // sphere center on F
60 Real m_radius_F{NaN}; // radius for F's sphere
61 Vec3 m_p_BSb{NaN}; // sphere center on B
62 Real m_radius_B{NaN}; // radius for B's sphere
63 };
64
65 struct PositionCache {
66 Transform X_AC; // contact frame in A
67 Vec3 p_FSf_A, p_BSb_A; // stations expressed in A
68 Vec3 p_FCo_A, p_BCo_A; // stations expressed in A
69 Vec3 p_SfSb_A; // vector from Sf to Sb, exp. in A
70 Real r, oor; // r=||p_SfSb_A||; oor=1/r (might be Infinity)
71 Real kf; // fraction along p_SfSb to find Co
72 bool isSingular; // if r is too small to be useful
73 };
74
75 struct VelocityCache {
76 Vec3 wXwX_p_FSf_A; // w_AF x (w_AF x p_FSf_A) [coriolis accel.]
77 Vec3 wXwX_p_BSb_A; // w_AB x (w_AB x p_BSb_A) [coriolis accel.]
78 Vec3 pd_SfSb_A; // v_ASb - vASf
79 Vec3 vF_BCo_A; // vA_BCo_A-vA_FCo_A, B/Co station vel in F
80 Vec3 wXpd_FCo_A; // w_AF % pd_FCo_A;
81 Vec3 wXpd_BCo_A; // w_AB % pd_BCo_A;
82 Vec3 Czd_A; // d/dt_A Cz_A -- depends on isSingular
83 Vec3 Cxd_A, Cyd_A; // d/dt_A Cx_A, Cy_A -- calculated from Czd_A
84 };
85
SphereOnSphereContactImpl(bool enforceRolling)86 explicit SphereOnSphereContactImpl(bool enforceRolling)
87 : ConstraintImpl(1, enforceRolling?2:0, 0),
88 m_enforceRolling(enforceRolling),
89 m_def_p_FSf(0), m_def_radius_F(NaN), m_def_p_BSb(0), m_def_radius_B(NaN)
90 { }
clone()91 SphereOnSphereContactImpl* clone() const override
92 { return new SphereOnSphereContactImpl(*this); }
93
94 void calcDecorativeGeometryAndAppendVirtual
95 (const State& s, Stage stage, Array_<DecorativeGeometry>& geom) const
96 override;
97
98 // Allocates the discrete state variable for the parameters, and the cache
99 // entries.
100 void realizeTopologyVirtual(State& state) const override;
101
102 // Get the current value of the runtime-settable parameters from this state.
103 const Parameters& getParameters(const State& state) const;
104
105 // Get a writable reference to the value of the discrete variable; this
106 // invalidated Position stage in the given state.
107 Parameters& updParameters(State& state) const;
108
109 const PositionCache& getPositionCache(const State& state) const;
110 PositionCache& updPositionCache(const State& state) const;
111 const PositionCache& ensurePositionCacheRealized(const State& state) const;
112
113 const VelocityCache& getVelocityCache(const State& state) const;
114 VelocityCache& updVelocityCache(const State& state) const;
115 const VelocityCache& ensureVelocityCacheRealized(const State& state) const;
116
117
118 // Implementation of virtuals required for holonomic constraints.
119
120 /* The normal contact condition is just a distance constraint enforcing that
121 the two center points Sf and Sb must exactly rf+rb apart. The scalar multiplier
122 acts along the center-center line, so the force application point doesn't
123 matter; it would be the same for any contact point along that line so we don't
124 need to calculate it explicitly for the normal contact constraint; it will
125 matter for tangential forces.
126 Let
127 P = Sf, Q = Sb
128 p_PQ = p_AQ - p_AP
129 pd_PQ = d/dt_A p_PQ = v_AQ - v_AP
130 pdd_PQ = d/dt_A pd_PQ = a_AQ - a_AP
131 r(q) = ||p_PQ|| [center-center distance]
132
133 There is a pathological case in which the center points of the two spheres
134 are overlapping, r(q)=0. In that case we can't use the center-center line as the
135 contact normal Cz since it is zero length. This condition will be corrected
136 eventually by assembly analysis; we just need some consistent return values
137 until then. In this case we declare that Cz=Fz:
138
139 (1) Cz = {p_PQ/r, r != 0 [contact normal]
140 { Fz, r == 0
141
142 Then our normal contact conditions can always be written like this:
143
144 (2) perr = r(q)-(rf+rb)
145 (3) verr = pd_PQ . Cz
146 (4) aerr = pdd_PQ . Cz + pd_PQ . d/dt_A Cz
147
148 In case r==0, Cz is fixed in F (it is Fz). So Czd = d/dt_A Cz = w_AF x Cz and
149 aerr = pdd_PQ . Cz + pd_PQ . (w_AF x Cz)
150 = pdd_PQ . Cz + (pd_PQ x w_AF) . Cz [triple product identity]
151 (5) = (pdd_PQ - w_AF x pd_PQ) . Cz [r==0]
152
153 Otherwise we need to calculate
154 Czd = d/dt_A Cz = d/dt_A p_PQ/r
155 = pd_PQ * oor + p_PQ * oord
156 = pd_PQ/r - p_PQ * (pd_PQ . p_PQ)/r^3
157 = pd_PQ/r - p_PQ/r * (pd_PQ . p_PQ/r)/r
158 = [pd_PQ - (pd_PQ . Cz)*Cz]/r
159 (6) = (pd_PQ - verr * Cz) / r
160
161 where
162 oor(q) = 1/r(q) = (p_PQ.p_PQ)^-1/2
163 oord(q) = d/dt oor(q) = -pd_PQ . p_PQ/r^3
164
165 Substituting (6) into (4) gives
166 aerr = pdd_PQ . Cz + pd_PQ . [pd_PQ - (pd_PQ . Cz)*Cz]/r
167 = pdd_PQ . Cz + [pd_PQ^2 - (pd_PQ . Cz)^2] / r
168 (7) = pdd_PQ . Cz + [pd_PQ^2 - verr^2] / r
169
170 The scalars returned by perr,verr, and aerr are measure numbers along Cz (a
171 unit vector aligned with p_SfSb). The multiplier will consequently act along Cz.
172
173 (See comments below for friction.)
174 */
175
176 /* --------------------------------
177 perr = ||p_SfSb|| - (rf+rb)
178 --------------------------------
179 ~60 flops
180
181 Note that the give State does not necessarily correspond to the given X_AB
182 and constrainedQ so we can't cache the results.
183
184 TODO: there ought to be a flag saying whether the X_AB and q are from the
185 state (which is common) so that the results could be saved.
186 */
calcPositionErrorsVirtual(const State & s,const Array_<Transform,ConstrainedBodyIndex> & allX_AB,const Array_<Real,ConstrainedQIndex> & constrainedQ,Array_<Real> & perr)187 void calcPositionErrorsVirtual
188 (const State& s, // Stage::Time
189 const Array_<Transform,ConstrainedBodyIndex>& allX_AB,
190 const Array_<Real, ConstrainedQIndex>& constrainedQ,
191 Array_<Real>& perr) // mp of these
192 const override
193 {
194 assert(allX_AB.size()==2 && constrainedQ.size()==0 && perr.size() == 1);
195
196 const Parameters& params = getParameters(s);
197 const Vec3& p_FSf = params.m_p_FSf;
198 const Vec3& p_BSb = params.m_p_BSb;
199 const Real rf = params.m_radius_F;
200 const Real rb = params.m_radius_B;
201
202 // 36 flops for these two together.
203 const Vec3 p_ASf = findStationLocation(allX_AB, m_mobod_F, p_FSf);
204 const Vec3 p_ASb = findStationLocation(allX_AB, m_mobod_B, p_BSb);
205
206 const Vec3 p_SfSb_A = p_ASb - p_ASf; // 3 flops
207 const Real r = p_SfSb_A.norm(); // ~20 flops
208
209 // Calculate this scalar using A-frame vectors; any frame would have done.
210 perr[0] = r - (rf+rb); // 2 flops
211 }
212
213
214 /* --------------------------------
215 verr = pd_SfSb_A . Cz
216 where pd_SfSb_A = v_ASb - v_ASf
217 and Cz = {p_SfSb_A / r, r!=0
218 {Fz_A, r==0
219 --------------------------------
220 32 flops if position info is already available.
221
222 Note that we can't cache the velocity results here because we don't know that
223 V_AB and qdot were taken from the given State.
224 */
calcPositionDotErrorsVirtual(const State & s,const Array_<SpatialVec,ConstrainedBodyIndex> & allV_AB,const Array_<Real,ConstrainedQIndex> & constrainedQDot,Array_<Real> & pverr)225 void calcPositionDotErrorsVirtual
226 (const State& s, // Stage::Position
227 const Array_<SpatialVec,ConstrainedBodyIndex>& allV_AB,
228 const Array_<Real, ConstrainedQIndex>& constrainedQDot,
229 Array_<Real>& pverr) // mp of these
230 const override
231 {
232 assert(allV_AB.size()==2 && constrainedQDot.size()==0 && pverr.size() == 1);
233 const PositionCache& pc = ensurePositionCacheRealized(s);
234 const UnitVec3& Cz_A = pc.X_AC.z();
235
236 const SpatialVec& V_AF = allV_AB[m_mobod_F];
237 const Vec3& w_AF = V_AF[0]; const Vec3& v_AF = V_AF[1];
238
239 const SpatialVec& V_AB = allV_AB[m_mobod_B];
240 const Vec3& w_AB = V_AB[0]; const Vec3& v_AB = V_AB[1];
241
242 const Vec3 v_ASf = v_AF + w_AF % pc.p_FSf_A; // 12 flops
243 const Vec3 v_ASb = v_AB + w_AB % pc.p_BSb_A; // 12 flops
244 const Vec3 pd_SfSb_A = v_ASb - v_ASf; // 3 flops
245 pverr[0] = ~pd_SfSb_A * Cz_A; // 5 flops
246 }
247
248 /* ------------------------------------------------------------
249 aerr = pdd_SfSb . Cz + pd_SfSb . Czd
250 where pdd_SfSb_A = a_ASb - a_ASf
251 and Cz = {p_SfSb_A / r, r!=0
252 {Fz_A, r==0
253 and Czd = d/dt_A Cz = {(pd_SfSb - Cz * (pd_SfSb . Cz))/r
254 {w_AF x Fz_A, r==0
255 ------------------------------------------------------------
256 44 flops if position & velocity info already calculated
257 */
calcPositionDotDotErrorsVirtual(const State & s,const Array_<SpatialVec,ConstrainedBodyIndex> & allA_AB,const Array_<Real,ConstrainedQIndex> & constrainedQDotDot,Array_<Real> & paerr)258 void calcPositionDotDotErrorsVirtual
259 (const State& s, // Stage::Velocity
260 const Array_<SpatialVec,ConstrainedBodyIndex>& allA_AB,
261 const Array_<Real, ConstrainedQIndex>& constrainedQDotDot,
262 Array_<Real>& paerr) // mp of these
263 const override
264 {
265 assert(allA_AB.size()==2&&constrainedQDotDot.size()==0&&paerr.size()==1);
266
267 // Ensuring the velocity cache is realized also ensures that the position
268 // cache has been realized.
269 const VelocityCache& vc = ensureVelocityCacheRealized(s);
270 const PositionCache& pc = getPositionCache(s);
271 const UnitVec3& Cz_A = pc.X_AC.z();
272
273 // 30 flops for these two together.
274 const Vec3 a_ASf = findStationInAAcceleration(s, allA_AB, m_mobod_F,
275 pc.p_FSf_A, vc.wXwX_p_FSf_A);
276 const Vec3 a_ASb = findStationInAAcceleration(s, allA_AB, m_mobod_B,
277 pc.p_BSb_A, vc.wXwX_p_BSb_A);
278
279 const Vec3 pdd_SfSb_A = a_ASb - a_ASf; // 2nd deriv in A, 3 flops
280 paerr[0] = ~pdd_SfSb_A * Cz_A + ~vc.pd_SfSb_A * vc.Czd_A; // 11 flops
281 }
282
283 // Because the normal force is applied along the line between the centers, it
284 // does not matter where along that line we apply the force -- the force and
285 // resulting moment are the same anywhere along the line. Thus we are not
286 // required to use the same point in space here -- for convenience we'll just
287 // apply forces to the sphere centers whose locations we have available.
288 // We won't have this luxury for tangential forces -- for those the application
289 // point does matter.
290 //
291 // apply f=lambda*Cz to the sphere center on body B,
292 // -f to the sphere center on body F
293 // 33 flops if position info already available.
addInPositionConstraintForcesVirtual(const State & s,const Array_<Real> & multipliers,Array_<SpatialVec,ConstrainedBodyIndex> & bodyForcesInA,Array_<Real,ConstrainedQIndex> & qForces)294 void addInPositionConstraintForcesVirtual
295 (const State& s, // Stage::Position
296 const Array_<Real>& multipliers, // mp of these
297 Array_<SpatialVec,ConstrainedBodyIndex>& bodyForcesInA,
298 Array_<Real, ConstrainedQIndex>& qForces)
299 const override
300 {
301 assert(multipliers.size()==1&&bodyForcesInA.size()==2&&qForces.size()==0);
302 const Real lambda = multipliers[0];
303
304 const PositionCache& pc = ensurePositionCacheRealized(s);
305 const UnitVec3& Cz_A = pc.X_AC.z();
306
307 const Vec3 force_A = lambda * Cz_A; // 3 flops
308
309 // 30 flops for the next two calls
310 addInStationInAForce(pc.p_BSb_A, force_A, bodyForcesInA[m_mobod_B]);
311 subInStationInAForce(pc.p_FSf_A, force_A, bodyForcesInA[m_mobod_F]);
312 }
313
314
315 // Implementation of virtuals required for nonholonomic constraints.
316
317 /* The rolling friction constraint says that the velocity in F of the material
318 point of B coincident with the contact point Co, must be zero in the contact
319 frame's Cx and Cy directions.
320
321 We have:
322 (1) p_FCo = p_FSf + k p_SfSb, k=rf/(rf+rb)
323 p_BCo = p_BSb + (k-1) p_SfSb
324 (2) = p_FCo - p_FB
325 Note that the contact point Co is not a station of either body but moves with
326 respect to the body frames F and B. However, for the velocity constraint
327 we want to enforce no relative tangential velocity of the two *material* points
328 at Co, which we'll call B/Co and F/Co and write
329 (3) vA_FCo = d/dt_A p_FCo
330 = v_AF + w_AF x p_FCo
331 (4) vA_BCo = d/dt A p_BCo
332 v_AB + w_AB x p_BCo
333 Then the velocity of B/Co measured in the F frame is
334 (5) vF_BCo = vA_BCo - vA_FCo
335 and
336 (6) verr = [vF_BCo . Cx]
337 [vF_BCo . Cy]
338 is our velocity constraint.
339
340 We need to calculate aerr = d/dt verr. The components of verr are two scalars
341 so we can pick any frame in which to calculate the derivatives; we'll use A. So
342 from the chain rule we have:
343 (7) aerr = [d/dt_A vF_BCo . Cx + vF_BCo . d/dt_A Cx]
344 [d/dt_A vF_BCo . Cy + vF_BCo . d/dt_A Cy]
345 = [vdF_BCo . Cx + vF_BCo . Cxd]
346 [vdF_BCo . Cy + vF_BCo . Cyd]
347
348 We already have vF_BCo, so we need these three quantities:
349 Cxd = d/dt_A Cx = w_AC x Cx
350 Cyd = d/dt_A Cy = w_AC x Cy
351 vFd_BCo = d/dt_A vF_BCo
352
353 We have to be careful to account for the fact that Co moves in B, that is
354 d/dt_B p_BCo != 0. We'll use "d" to mean time derivative in A:
355 vdF_BCo = vdA_BCo - vdA_FCo from (5)
356 vdA_FCo = a_AF + b_AF x p_FCo + w_AF x pd_FCo from (3)
357 vdA_BCo = a_AB + b_AB x p_BCo + w_AB x pd_BCo from (4)
358 pd_FCo = w_AF x p_FSf + kf * pd_SfSb from (1)
359 pd_BCo = pdFCo - pd_FB from (2)
360 pd_FB = v_AB - v_AF
361
362 You can find the velocity-dependent portions of the above calculations,
363 including Cxd and Cyd, in ensureVelocityCacheRealized().
364 */
365
366 /*
367 -------------------------------------------
368 verr = [vF_BCo_A . Cx_A]
369 [vF_BCo_A . Cy_A]
370 -------------------------------------------
371 37 flops (if position info already calculated)
372 */
calcVelocityErrorsVirtual(const State & s,const Array_<SpatialVec,ConstrainedBodyIndex> & allV_AB,const Array_<Real,ConstrainedUIndex> & constrainedU,Array_<Real> & verr)373 void calcVelocityErrorsVirtual
374 (const State& s, // Stage::Position
375 const Array_<SpatialVec,ConstrainedBodyIndex>& allV_AB,
376 const Array_<Real, ConstrainedUIndex>& constrainedU,
377 Array_<Real>& verr) // mv of these
378 const override
379 {
380 assert(allV_AB.size()==2 && constrainedU.size()==0 && verr.size()==2);
381
382 const PositionCache& pc = ensurePositionCacheRealized(s);
383 const UnitVec3& Cx_A = pc.X_AC.x();
384 const UnitVec3& Cy_A = pc.X_AC.y();
385
386 const SpatialVec& V_AF = allV_AB[m_mobod_F];
387 const Vec3& w_AF = V_AF[0]; const Vec3& v_AF = V_AF[1];
388
389 const SpatialVec& V_AB = allV_AB[m_mobod_B];
390 const Vec3& w_AB = V_AB[0]; const Vec3& v_AB = V_AB[1];
391
392 const Vec3 vA_BCo_A = v_AB + w_AB % pc.p_BCo_A; // 12 flops
393 const Vec3 vA_FCo_A = v_AF + w_AF % pc.p_FCo_A; // 12 flops
394 const Vec3 vF_BCo_A = vA_BCo_A - vA_FCo_A; // 3 flops
395
396 // Calculate these scalars using A-frame vectors, but the results are
397 // measure numbers in [Cx Cy].
398 verr[0] = ~vF_BCo_A * Cx_A; // 5 flops
399 verr[1] = ~vF_BCo_A * Cy_A; // 5 flops
400 }
401
402 /*
403 -------------------------------------------
404 aerr = [vdF_BCo . Cx + vF_BCo . Cxd]
405 [vdF_BCo . Cy + vF_BCo . Cyd]
406 -------------------------------------------
407 55 flops if position and velocity info already calculated.
408 */
409
calcVelocityDotErrorsVirtual(const State & s,const Array_<SpatialVec,ConstrainedBodyIndex> & allA_AB,const Array_<Real,ConstrainedUIndex> & constrainedUDot,Array_<Real> & vaerr)410 void calcVelocityDotErrorsVirtual
411 (const State& s, // Stage::Velocity
412 const Array_<SpatialVec,ConstrainedBodyIndex>& allA_AB,
413 const Array_<Real, ConstrainedUIndex>& constrainedUDot,
414 Array_<Real>& vaerr) // mv of these
415 const override
416 {
417 assert(allA_AB.size()==2 && constrainedUDot.size()==0 && vaerr.size()==2);
418
419 // Ensuring the velocity cache is realized also ensures that the position
420 // cache has been realized.
421 const VelocityCache& vc = ensureVelocityCacheRealized(s);
422 const PositionCache& pc = getPositionCache(s);
423
424 const UnitVec3& Cx_A = pc.X_AC.x();
425 const UnitVec3& Cy_A = pc.X_AC.y();
426 const Vec3& Cxd_A = vc.Cxd_A;
427 const Vec3& Cyd_A = vc.Cyd_A;
428
429 const SpatialVec& A_AF = allA_AB[m_mobod_F];
430 const Vec3& b_AF = A_AF[0]; const Vec3& a_AF = A_AF[1];
431
432 const SpatialVec& A_AB = allA_AB[m_mobod_B];
433 const Vec3& b_AB = A_AB[0]; const Vec3& a_AB = A_AB[1];
434
435 const Vec3 vd_FCo_A = a_AF + b_AF % pc.p_FCo_A + vc.wXpd_FCo_A; // 15 flops
436 const Vec3 vd_BCo_A = a_AB + b_AB % pc.p_BCo_A + vc.wXpd_BCo_A; // 15 flops
437 const Vec3 vdF_BCo_A = vd_BCo_A - vd_FCo_A; // 3 flops
438
439 vaerr[0] = ~vdF_BCo_A*Cx_A + ~vc.vF_BCo_A*Cxd_A; // 11 flops
440 vaerr[1] = ~vdF_BCo_A*Cy_A + ~vc.vF_BCo_A*Cyd_A; // 11 flops
441 }
442
443 // apply f=lambda0*Px + lambda1*Py to the bottom point C of ball on B
444 // -f to point C (coincident point) of body F
445 // 39 flops if position info already available
addInVelocityConstraintForcesVirtual(const State & s,const Array_<Real> & multipliers,Array_<SpatialVec,ConstrainedBodyIndex> & bodyForcesInA,Array_<Real,ConstrainedUIndex> & mobilityForces)446 void addInVelocityConstraintForcesVirtual
447 (const State& s, // Stage::Velocity
448 const Array_<Real>& multipliers, // mv of these
449 Array_<SpatialVec,ConstrainedBodyIndex>& bodyForcesInA,
450 Array_<Real, ConstrainedUIndex>& mobilityForces)
451 const override
452 {
453 assert(multipliers.size()==2 && mobilityForces.size()==0
454 && bodyForcesInA.size()==2);
455 const Real lambda0 = multipliers[0], lambda1 = multipliers[1];
456
457 const PositionCache& pc = ensurePositionCacheRealized(s);
458 const UnitVec3& Cx_A = pc.X_AC.x();
459 const UnitVec3& Cy_A = pc.X_AC.y();
460
461 const Vec3 force_A = lambda0*Cx_A + lambda1*Cy_A; // 9 flops
462
463 // 30 flops for the next two calls
464 addInStationInAForce(pc.p_BCo_A, force_A, bodyForcesInA[m_mobod_B]);
465 subInStationInAForce(pc.p_FCo_A, force_A, bodyForcesInA[m_mobod_F]);
466 }
467
468
469 SimTK_DOWNCAST(SphereOnSphereContactImpl, ConstraintImpl);
470 //------------------------------------------------------------------------------
471 private:
472 friend class Constraint::SphereOnSphereContact;
473
474 // These can't be changed after construction.
475 ConstrainedBodyIndex m_mobod_F; // F (B1)
476 ConstrainedBodyIndex m_mobod_B; // B (B2)
477 const bool m_enforceRolling; // if so, add 2 constraints
478
479 Vec3 m_def_p_FSf; // default sphere center on F
480 Real m_def_radius_F; // default radius for F's sphere
481 Vec3 m_def_p_BSb; // default sphere center on B
482 Real m_def_radius_B; // default radius for B's sphere
483
484 // This Position-stage variable holds the constraint parameters to be used
485 // for a particular State.
486 mutable DiscreteVariableIndex m_parametersIx;
487
488 // These cache entries hold precalculated position- and velocity-dependent
489 // computations. These are lazy evaluated by the first method to be called
490 // that requires the State to be at Position or Velocity stage.
491 mutable CacheEntryIndex m_posCacheIx;
492 mutable CacheEntryIndex m_velCacheIx;
493 };
494
495
496
497 } // namespace SimTK
498
499 #endif // SimTK_SIMBODY_CONSTRAINT_SPHERE_ON_SPHERE_CONTACT_IMPL_H_
500
501
502
503