public override THREE.Vector3 Constrain(THREE.Vector3 pos)
{
THREE.Vector3 diff = DivVector.sub(pos, Center).clone();
if (diff.length() < Radius)
{
double dist = Radius - diff.length();
diff.normalize().multiplyScalar(dist);
return(diff);
}
return(null);
}
public virtual void satisify()
{
THREE.Vector3 diff = DivVector.sub(p2.position, p1.position);
var currentLength = diff.length();
if (currentLength == 0)
{
return; // prevents division by 0
}
setPosition();
}
private void setPosition()
{
THREE.Vector3 diff = p2.position.clone().sub(p1.position);
THREE.Vector3 correctionHalf = null;
var currentLength = diff.length();
double stiff = 0;
if (currentLength == 0)
{
return;
}
double delta = currentLength - restLength;
if (delta < 0)
{
return;
}
stiff = restLength / currentLength;
var correction = diff.normalize().multiplyScalar(delta);
double mid = p1.IsFixed || p2.IsFixed ? 1.0 : 0.5;
correctionHalf = correction.multiplyScalar(mid);
if (!p1.IsFixed)
{
p1.position.add(correctionHalf);
}
if (!p2.IsFixed)
{
p2.position.sub(correctionHalf);
}
}