56 m_localAnchorB{def.bodyB?
59 m_frequency{def.frequency},
60 m_dampingRatio{def.dampingRatio},
61 m_maxForce{def.maxForce}
64 assert(
IsValid(def.dampingRatio));
80 if (m_targetA != target)
94 const auto invMass = body.GetInvMass();
95 const auto invRotInertia = body.GetInvRotInertia();
109 void TargetJoint::InitVelocityConstraints(
BodyConstraintsMap& bodies,
const StepConf& step,
110 const ConstraintSolverConf&)
112 auto& bodyConstraintB =
At(bodies,
GetBodyB());
114 const auto posB = bodyConstraintB->GetPosition();
115 auto velB = bodyConstraintB->GetVelocity();
122 const auto omega =
Real{2} *
Pi * m_frequency;
125 const auto d =
Real{2} * mass * m_dampingRatio * omega;
128 const auto k = mass *
Square(omega);
133 const auto h = step.GetTime();
134 const auto tmp = d + h * k;
137 const auto invGamma =
Mass{h * tmp};
138 m_gamma = (invGamma != 0_kg)?
Real{1} / invGamma:
InvMass{0};
139 const auto beta =
Frequency{h * k * m_gamma};
142 m_rB =
Rotate(m_localAnchorB - bodyConstraintB->GetLocalCenter(), qB);
144 m_mass = GetEffectiveMassMatrix(*bodyConstraintB);
150 velB.angular *= 0.98f;
152 if (step.doWarmStart)
154 m_impulse *= step.dtRatio;
155 const auto P = m_impulse;
157 velB += Velocity{bodyConstraintB->GetInvMass() * P, bodyConstraintB->GetInvRotInertia() * crossBP};
164 bodyConstraintB->SetVelocity(velB);
167 bool TargetJoint::SolveVelocityConstraints(
BodyConstraintsMap& bodies,
const StepConf& step)
169 auto& bodyConstraintB =
At(bodies,
GetBodyB());
171 auto velB = bodyConstraintB->GetVelocity();
176 const auto oldImpulse = m_impulse;
177 const auto addImpulse =
Transform(-ev, m_mass);
179 m_impulse += addImpulse;
180 const auto maxImpulse = step.GetTime() *
Force{m_maxForce};
186 const auto incImpulse = (m_impulse - oldImpulse);
190 bodyConstraintB->GetInvMass() * incImpulse,
191 bodyConstraintB->GetInvRotInertia() * angImpulseB
194 bodyConstraintB->SetVelocity(velB);
199 bool TargetJoint::SolvePositionConstraints(
BodyConstraintsMap& bodies,
const ConstraintSolverConf& conf)
const
228 m_targetA -= newOrigin;