public override void Update(double deltaTimeInSeconds)
{
this.currentTime += deltaTimeInSeconds;
this.CalcCurrentFrameIndex();
var alpha = (float)((this.currentTime - this.wayPoints[currentFrame].Time) /
(this.wayPoints[currentFrame + 1].Time - this.wayPoints[currentFrame].Time));
alpha = DigitExtensions.Clamp(alpha, 0f, 1f);
var curPos = this.wayPoints[currentFrame].Pos;
var curTan = this.wayPoints[currentFrame].Tangent;
var curRot = this.wayPoints[currentFrame].Rot;
var nextPos = this.wayPoints[currentFrame + 1].Pos;
var nextTan = this.wayPoints[currentFrame + 1].Tangent;
var nextRot = this.wayPoints[currentFrame + 1].Rot;
var newPosX = this.HermiteCurve(curPos.X, nextPos.X, curTan.X, nextTan.X, alpha);
var newPosY = this.HermiteCurve(curPos.Y, nextPos.Y, curTan.Y, nextTan.Y, alpha);
var newPosZ = this.HermiteCurve(curPos.Z, nextPos.Z, curTan.Z, nextTan.Z, alpha);
var newQuat = SCNQuaternion.Slerp(curRot, nextRot, alpha);
node.WorldPosition = new SCNVector3(newPosX, newPosY, newPosZ);
node.WorldOrientation = newQuat;
// update child rigid bodies to percolate into physics
if (this.Entity is GameObject entity)
{
if (entity.PhysicsNode?.PhysicsBody != null)
{
entity.PhysicsNode.PhysicsBody.ResetTransform();
}
}
}
public void Apply(PhysicsNodeData nodeData, bool isHalfway)
{
if (this.PhysicsNode != null)
{
// if we're not alive, avoid applying physics updates.
// this will allow objects on clients to get culled properly
if (this.IsAlive)
{
if (isHalfway)
{
this.PhysicsNode.WorldPosition = (nodeData.Position + this.PhysicsNode.WorldPosition) * 0.5f;
this.PhysicsNode.Orientation = SCNQuaternion.Slerp(this.PhysicsNode.Orientation, nodeData.Orientation, 0.5f);
}
else
{
this.PhysicsNode.WorldPosition = nodeData.Position;
this.PhysicsNode.Orientation = nodeData.Orientation;
}
if (this.PhysicsNode.PhysicsBody != null)
{
this.PhysicsNode.PhysicsBody.ResetTransform();
this.PhysicsNode.PhysicsBody.Velocity = nodeData.Velocity;
this.PhysicsNode.PhysicsBody.AngularVelocity = nodeData.AngularVelocity;
}
}
}
}
/// <summary>
/// Inch geometry back to original offset from rigid body
/// </summary>
private void UpdateSmooth(double deltaTime)
{
// allow some motion up to a maximum offset
var posDelta = this.parentOffPos - this.sourceOffPos;
if (posDelta.Length > MaxCorrection)
{
posDelta = MaxCorrection * SCNVector3.Normalize(posDelta);
}
// lerp pos
var newPos = this.sourceOffPos + posDelta;
this.geometryNode.Position = newPos;
// cap the max rotation that can show through
var quatDelta = this.parentOffRot.Divide(this.sourceOffRot);
quatDelta.ToAxisAngle(out SCNVector3 _, out float angle);
if (angle > MaxRotation)
{
this.geometryNode.Orientation = SCNQuaternion.Slerp(this.sourceOffRot, this.parentOffRot, MaxRotation / angle);
}
else
{
this.geometryNode.Orientation = this.parentOffRot;
}
}
public static SCNMatrix4 Interpolate(SCNMatrix4 scnm0, SCNMatrix4 scnmf, nfloat factor)
{
SCNVector4 p0 = scnm0.Row3;
SCNVector4 pf = scnmf.Row3;
var q0 = scnm0.ToQuaternion();
var qf = scnmf.ToQuaternion();
SCNVector4 pTmp = Lerp(p0, pf, factor);
SCNQuaternion qTmp = SCNQuaternion.Slerp(q0, qf, (float)factor);
SCNMatrix4 rTmp = qTmp.ToMatrix4();
SCNMatrix4 transform = new SCNMatrix4(
rTmp.M11, rTmp.M12, rTmp.M13, 0.0f,
rTmp.M21, rTmp.M22, rTmp.M23, 0.0f,
rTmp.M31, rTmp.M32, rTmp.M33, 0.0f,
pTmp.X, pTmp.Y, pTmp.Z, 1.0f
);
return(transform);
}
