public override bool getGuidance(Vector3D pos, ref Vector3D guide, ref float weight, float height)
{
if (!base.getGuidance(pos, ref guide, ref weight, height))
{
return(false);
}
// size: 5x2x1, meaning 12.5 x 5 x 2.5, slope of -1/5
// rail begins at x=-6.25 y=1.25 and goes to x=6.25 y=-1.25
var localCoords = Vector3D.Transform(pos, this.cubeBlock.WorldMatrixNormalizedInv);
var unrotatedCoords = Vector3D.Transform(localCoords, ref this.adjustMatrix);
// MyLog.Default.WriteLine(String.Format("angle of {0} turns {1} to {2}", Math.Acos(5 / Math.Sqrt(5*5 + 1*1)), localCoords, unrotatedCoords));
var length = (float)Math.Sqrt(5 * 5 + 1 * 1) * 2.5f;
return(StraightRailGuide.straight_guidance(length, this.unadjustMatrix * this.cubeBlock.WorldMatrix, unrotatedCoords,
ref guide, ref weight, height));
}
public override bool getGuidance(Vector3D pos, ref Vector3D guide, ref float weight, float height)
{
if (!base.getGuidance(pos, ref guide, ref weight, height))
{
return(false);
}
var localCoords = Vector3D.Transform(pos, this.cubeBlock.WorldMatrixNormalizedInv);
// MyLog.Default.WriteLine(String.Format("local coord is {0} [{1}]", localCoords, flip_curve_z));
bool tracking = false;
var tangent = divider_pt2 - divider_pt1;
var normal = new Vector3D(tangent.Z, 0, -tangent.X); // 90° rotated
// determine on which side of the divider we are
var right_side = ((localCoords - divider_pt1) * new Vector3D(1, 0, 1)).Dot(normal) > 0;
if (right_side)
{
Vector3D curve_guide = new Vector3D(); float curve_weight = 0.0f;
var curveCoords = localCoords;
var curveWorldMat = this.cubeBlock.WorldMatrix;
if (flip_curve_z)
{
var invZ = new Vector3D(1, 1, -1);
curveCoords *= invZ;
MatrixD.Rescale(ref curveWorldMat, ref /* WHY */ invZ);
}
bool outer_curve_was_picked;
Curve90_10x_12x_RailGuide.curved_guidance(
curveCoords,
curveWorldMat,
out outer_curve_was_picked,
ref curve_guide, ref curve_weight, height,
lean: false
);
Vector3D straight_guide = new Vector3D(); float straight_weight = 0.0f;
StraightRailGuide.straight_guidance(
7 * 1.25f,
straight_outer_long_to_junction * this.cubeBlock.WorldMatrix,
Vector3D.Transform(localCoords, junction_to_straight_outer_long),
ref straight_guide, ref straight_weight, height
);
bool inner_straight_was_picked = StraightRailGuide.straight_guidance(
8 * 1.25f,
straight_inner_long_to_junction * this.cubeBlock.WorldMatrix,
Vector3D.Transform(localCoords, junction_to_straight_inner_long),
ref straight_guide, ref straight_weight, height
);
Vector3D picked_guide = Vector3D.Zero; float picked_weight = 0;
MatrixD worldMat = this.cubeBlock.WorldMatrix, worldMatInv = this.cubeBlock.WorldMatrixNormalizedInv;
bool curve_guide_was_picked;
pick_appropriate_guides(
pos, localCoords,
ref worldMat, ref worldMatInv,
curve_guide, curve_weight,
straight_guide, straight_weight,
out curve_guide_was_picked,
ref picked_guide, ref picked_weight
);
// DebugDraw.Sphere(picked_guide / picked_weight, 0.2f, Color.Yellow);
// inner rails are "passive" (meaning they only apply Y correction)
// if they are active, we rely on the outer rail for XZ guidance
// (but we still have to do the pick process so we don't break the snapping)
if (curve_guide_was_picked && outer_curve_was_picked ||
!curve_guide_was_picked && inner_straight_was_picked
)
{
picked_guide = new Vector3D(localCoords.X, height - 1.25, localCoords.Z);
picked_guide = Vector3D.Transform(picked_guide, worldMat) * picked_weight;
}
guide += picked_guide;
weight += picked_weight;
tracking |= picked_weight > 0;
}
// from swivel space into parent local space, recentering the rail piece for straight_guidance
var swivel_long_mat = MatrixD.CreateTranslation(1.25 * 4, 0, 0) * this.cubeBlock.GetSubpart(swivel_long).PositionComp.LocalMatrix;
var swivel_short_mat = MatrixD.CreateTranslation(1.25 * 3, 0, 0) * this.cubeBlock.GetSubpart(swivel_short).PositionComp.LocalMatrix;
var sw_long_world_mat = swivel_long_mat * this.cubeBlock.WorldMatrix;
var sw_short_world_mat = swivel_short_mat * this.cubeBlock.WorldMatrix;
tracking |= StraightRailGuide.straight_guidance(
4 * 1.25f,
sw_long_world_mat,
Vector3D.Transform(pos, MatrixD.Invert(sw_long_world_mat)),
// subpart rails are at 0, but straight_guidance thinks they are at -1.25
// so to float height above them, float height + 1.25 above where guidance thinks they are.
ref guide, ref weight, height + 1.25f
);
tracking |= StraightRailGuide.straight_guidance(
3 * 1.25f,
sw_short_world_mat,
Vector3D.Transform(pos, MatrixD.Invert(sw_short_world_mat)),
ref guide, ref weight, height + 1.25f
);
tracking |= StraightRailGuide.straight_guidance(
1.25f,
straight_outer_short_to_junction * this.cubeBlock.WorldMatrix,
Vector3D.Transform(localCoords, junction_to_straight_outer_short),
ref guide, ref weight, height,
apply_overhang: false
);
tracking |= StraightRailGuide.straight_guidance(
1.25f,
straight_inner_short_to_junction * this.cubeBlock.WorldMatrix,
Vector3D.Transform(localCoords, junction_to_straight_inner_short),
ref guide, ref weight, height,
apply_overhang: false
);
return(tracking);
}
