# yade-users team mailing list archive

## Re: [Question #698572]: Is it possible to constrain the motion of a body along a circle

```Question #698572 on Yade changed:

Jan Stránský proposed the following answer:
My "velocity" contribution (I could not resist and tried the "velocity"
approach :-)

Recalling the idea:
- after InteractionLoop before NewtonIntegrator, forces (i.e. acceleration) of bodies are known
- pre-compute positions, that NewtonIntegrator would compute
- constrain this pre-computed position
- "trick" the velocity such that the actual NewtonIntegrator computes the desired constrained position

For linear components, first velocity is updated and then position is updated:
vel_next = vel_prev + accel * dt
pos_next = pos_prev + vel_next * dt = pos_prev + (vel_prev + accel*dt)*dt =
= pos_prev + vel_prev*dt + accel*dt*dt

Knowing pos_next, constrain it: pos_next -> pos_next_constrained (e.g. closest point projection)
Rewrite the pos_next equation for pos_next_constrained:
pos_next_constrained =pos_prev + vel_prev*dt + accel*dt*dt
Solve for vel_prev:
vel_prev = (pos_next_constrained - pos_prev) / dt - accel*dt

In your specific case, the orientation is directly related to position.
But I think that for a general case, it should be possible to "trick" the angular velocity similarly

Code
====

Approach 1, linear velocity only (results in pedal-like motion):
###
sph1 = sphere((0, -2, 2), .5)
sph2 = sphere((0, 0, 0), .5)
clump_id,(sph1_id,sph2_id) = O.bodies.appendClumped((sph1,sph2))
clump = O.bodies[clump_id]
O.engines = [
ForceResetter(),
InsertionSortCollider([Bo1_Sphere_Aabb()]),
InteractionLoop(
[Ig2_Sphere_Sphere_ScGeom()],
[Ip2_FrictMat_FrictMat_FrictPhys()],
[Law2_ScGeom_FrictPhys_CundallStrack()]
),
PyRunner(command = "eccentricAxisEngine()", iterPeriod = 1),
NewtonIntegrator(gravity=(0,0,-10), damping=0, label="newton"),
]
O.dt = .5e-2 * PWaveTimeStep()

def constraintClosesPoint(pos):
delta = pos - center
direction = delta.normalized()
return  center + direction * radius
def eccentricAxisEngine():
dt = O.dt
s = clump.state
f = O.forces.f(clump_id)
a = f / s.mass + newton.gravity
pos1 = s.pos + (s.vel+a*dt)*dt
pos0 = constraintClosesPoint(pos1)
s.vel = (pos0 - s.pos) / dt - a*dt
###

Approach 2, linear + angular velocity (perfect results):
###
sph1 = sphere((0, -2, 2), .5)
sph2 = sphere((0, 0, 0), .5)
clump_id,(sph1_id,sph2_id) = O.bodies.appendClumped((sph1,sph2))
clump = O.bodies[clump_id]
O.engines = [
ForceResetter(),
InsertionSortCollider([Bo1_Sphere_Aabb()]),
InteractionLoop(
[Ig2_Sphere_Sphere_ScGeom()],
[Ip2_FrictMat_FrictMat_FrictPhys()],
[Law2_ScGeom_FrictPhys_CundallStrack()]
),
PyRunner(command = "eccentricAxisEngine()", iterPeriod = 1),
NewtonIntegrator(gravity=(0,0,-10), damping=0, label="newton"),
]
O.dt = .5e-2 * PWaveTimeStep()

clump.state.refOri = clump.state.ori # not done automatically
clump.state.blockedDOFs = "XYZ" # to force "simple" angular integration

def constraintClosesPoint(pos):
delta = pos - center
direction = delta.normalized()
pos = center + direction * radius
angle = atan2(direction[2],direction[1])
ori = Quaternion()
ori.setFromTwoVectors(Vector3.UnitX,direction)
return pos,ori
def eccentricAxisEngine():
dt = O.dt
s = clump.state
f = O.forces.f(clump_id)
a = f / s.mass + newton.gravity
pos1 = s.pos + (s.vel+a*dt)*dt
pos0,ori0 = constraintClosesPoint(pos1)
s.vel = (pos0 - s.pos) / dt - a*dt
#
v1 = s.ori * Vector3.UnitX
v2 = ori0 * Vector3.UnitX
ang = v1.cross(v2)
ang = ang / O.dt
s.angVel = ang
###

Cheers
Jan

--