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); }