USARBot.KSlider

//=================================================
// This class simulates a telescopic joint.
// 
// Approach: the telescopic movemeent is simulated by a tow-bar linkage showed below:
//                        o
//                       / \
//                Bar1  /   \ Bar2
//                     /     \
//            Part1 I=o=I   I=o=I  Part2
//
//           By turning Bar1 and Bar2 together we can generate the tranlation movement
//           between Part1 and Part2.
//
// Usage: We use KSlider as a normal Joint. In the current version, there are some limitations
//        because we must use the ParentPos and SelfPos in the ini file to define how 
//        Part1 and Part2 are connected and the maximal telescopic distance. 
//        1) In the ini file we always define the extreme situation of maximum telescopic distance.
//        2) The ParentPos and SelfPos are used to calculate the Part2's location relative to Part1.
//        3) The inner joint of Bar1 and Part1 is always on the CENTER of Part1. 
//           And the same happens with Bar2 and Part2. The Joint of Bar1 and Bar2 is placed on 
//           the middle position of the two previous joints.
//        4) It's the user's resposibility to carefully define the collision meshes for Part1 and Part2
//           to ensure no collision will happen during the translation movement.
//        *HINT:* Carefully defining the center of Part1 and Part2 will make the configuration much easier.
//                I suggest you do it in this way: under the case of zero telescopic distance, we define
//                the center of Part1 and Part2 spatially overlay to each other. Therefore, the ParentPos
//                will be (0,0,0) and SelfPos is (d,0,0) where 'd' is the maximum telescopic distance.
//        
//        In the future, if we use Joint type in the configuration as I previously suggested, we will be able 
//        to directly define the telescopic capability in the type definition. Then we will be able to use 
//        KSlider exactly as a normal joint. In the ini file we define the case of zero telescopic distance,
//        and the first inner joint is defined by ParentPos and the second is defined by SelfPos. They overlay
//        to each other by default. And the middle joint will be palce as perpendicular to Part1 and Part2. 
//        The distance between middle joint and the first/second joint will be half of the max telescopic distance.
//
// Control: DRIVE {Name xxx} {Value xxx} {Order xx}
//          where Name is KSlider's name; Value is the telescopic distance in meter, and Order can be 0 or 10
//          that in turn represent absoluate and relative translation control.
//=================================================

class KSlider extends KDHinge;

var float DesiredDistance;

var float barLength;
var bool bInProtection;
var float lastCA, midLastCA, termLastCA;
var float spinSpeed, midSpinSpeed, termSpinSpeed;

var KActor Part1, Part2, Part;
var KDHinge MidJoint, TermJoint;

simulated function init1(Actor a1, Vector p1, Vector ax1, Vector sax1, Vector p2, Vector ax2, Vector sax2)
{
  local vector RotX, RotY, RotZ, offset;
  
  Part1 = spawn(class'USARBot.SlidePart',a1,,a1.Location,a1.Rotation);
  KConstraintActor1 = a1;
  KPos1 = vect(0,0,0);
  KPriAxis1 = ax1;
  KSecAxis1 = sax1;
  KConstraintActor2 = Part1;
  KPos2 = vect(0,0,0);
  KPriAxis2 = ax2;
  KsecAxis2 = sax2;
  SetPhysics(PHYS_Karma);
  
  GetAxes(a1.Rotation,RotX,RotY,RotZ);
  offset = p1 - p2;
  Part2 = spawn(class'USARBot.SlidePart', Part1,, a1.Location + offset.X*RotX + offset.Y*RotY + offset.Z*RotZ,a1.Rotation);
  MidJoint = spawn(class'USARBot.KDHinge',Part1);
  MidJoint.KConstraintActor1 = Part1;
  MidJoint.KPos1 = offset/100;
  MidJoint.KPriAxis1 = ax1;
  MidJoint.KSecAxis1 = sax1;
  MidJoint.KConstraintActor2 = Part2;
  MidJoint.KPos2 = offset/-100;
  MidJoint.KPriAxis2 = ax2;
  MidJoint.KSecAxis2 = sax2;
  MidJoint.SetPhysics(PHYS_Karma);

  offset.X *= 1-ax1.X;
  offset.Y *= 1-ax1.Y;
  offset.Z *= 1-ax1.Z;
  barLength = VSize(offset)/2;
}
  
simulated function init2(Vector p1, Vector ax1, Vector sax1, Actor a2, Vector p2, Vector ax2, Vector sax2)
{
  TermJoint = spawn(class'USARBot.KDHinge',Part2);
  TermJoint.KConstraintActor1 = Part2;
  TermJoint.KPos1 = vect(0,0,0);
  TermJoint.KPriAxis1 = ax1;
  TermJoint.KSecAxis1 = sax1;
  TermJoint.KConstraintActor2 = a2;
  TermJoint.KPos2 = vect(0,0,0);
  TermJoint.KPriAxis2 = ax2;
  TermJoint.KSecAxis2 = sax2;
  TermJoint.SetPhysics(PHYS_Karma);
  
  lastCA = KCurrentAngle;
  MidLastCA = MidJoint.KCurrentAngle;
  TermLastCA = TermJoint.KCurrentAngle;
  
  SetTimer(1,true);
}

simulated function Update()
{
  UpdateConstraint();
  KConstraintActor1.KWake();
}

simulated function float getDistance()
{
  return 2*barLength*Cos(PI*KCurrentAngle/32768);
}

simulated function UpdateConstraint()
{
  //log("DesiredDistance="$DesiredDistance);
  if (DesiredDistance>2*barLength)  
    DesiredDistance = 2*barLength;
  else if (DesiredDistance<0)
    DesiredDistance = 0;
  KDesiredAngle = -Acos(DesiredDistance/(2*barLength))*32768/PI; //absolute angle
  KUpdateConstraintParams();

  MidJoint.KDesiredAngle = -2*KDesiredAngle;
  MidJoint.KMaxTorque = 2*KMaxTorque;
  if (KHingeType==HT_Motor)
    MidJoint.KDesiredAngVel = -2*KDesiredAngVel;
  else
    MidJoint.KDesiredAngVel = 2*KDesiredAngVel;
  MidJoint.KProportionalGap = KProportionalGap;
  MidJoint.KHingeType = KHingeType;
  MidJoint.KUpdateConstraintParams();

  TermJoint.KDesiredAngle = KDesiredAngle;
  TermJoint.KMaxTorque = KMaxTorque;
  TermJoint.KDesiredAngVel = KDesiredAngVel;
  TermJoint.KProportionalGap = KProportionalGap;
  TermJoint.KHingeType = KHingeType;
  TermJoint.KUpdateConstraintParams();
}

simulated function Tick(float delta)
{
  if (!bInProtection && (KCurrentAngle>16384 || KCurrentAngle<-16384)) {
    bInProtection = True;
    KHingeType = HT_Controlled;
    if (KCurrentAngle>16384)
      KDesiredAngle = 16380;
    else
      KDesiredAngle = -16380;
    KDesiredAngVel = 4000;
    UpdateConstraint();
    //log("Adj:"@KDesiredAngle@KDesiredAngVel);
  }
  if (bInProtection && KCurrentAngle<16384 && KCurrentAngle>-16384)
    bInProtection = false;
  //log(DesiredDistance@getDistance());
}

// adjust the joint spin speed to ensure the synchronized joints movement 
simulated function timer()
{
  spinSpeed = KCurrentAngle - lastCA;
  MidSpinSpeed = MidJoint.KCurrentAngle - MidLastCA;
  TermSpinSpeed = TermJoint.KCurrentAngle - TermLastCA;
  
  if (Abs(KDesiredAngle-KCurrentAngle)>200 && Abs(MidSpinSpeed/spinSpeed+2)>0.05) {
    if (KHingeType==HT_Motor)
        MidJoint.KDesiredAngVel = KDesiredAngVel*4*spinSpeed/MidSpinSpeed;
    else
        MidJoint.KDesiredAngVel = -KDesiredAngVel*4*spinSpeed/MidSpinSpeed;
    MidJoint.KUpdateConstraintParams();
    //log(Abs(KDesiredAngle-KCurrentAngle)@spinSpeed@MidSpinSpeed@MidJoint.KDesiredAngVel);
  }
  
  lastCA = KCurrentAngle;
  MidLastCA = MidJoint.KCurrentAngle;
  TermLastCA = TermJoint.KCurrentAngle;
}

simulated event Destroyed()
{
  Part2.Destroy();
  TermJoint.Destroy();
  Part1.Destroy();
  MidJoint.Destroy();
  Super.Destroyed();
}