Marlin code for scissor lift z-axis

I'm trying to figure out where in marlin I might modify the Z axis so that it follows a non-linear path for a scissor lift. The formula is quite simple and thanks to wolfmanjm's help in the IRC channel, I think I figured out what I should change.



The formula that needs to be plugged in is above, and in Marlin's planner.cpp, there are these few lines below;

 // The target position of the tool in absolute steps
  // Calculate target position in absolute steps
  //this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow
  long target[4];
  target[X_AXIS] = lround(x*axis_steps_per_unit[X_AXIS]);
  target[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]);
  target[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]); 
  target[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]);

Since I have no idea on how to code properly, is there anyone who can help me with the proper syntax for the formula above?

My own rough attempt results in something like this;

  target[Z_AXIS] = 1round(sqrt((sq(Scissor_arm_length)-sq(y*axis_steps_per_unit[Z-AXIS])));

Can anyone help me with this little coding problem?