private void Awake()
{
ikChain = GetComponent <IKChain>();
rootJoint = ikChain.getRootJoint();
timeSinceLastStep = 2 * stepCooldown; // Make sure im allowed to step at start
timeStandingStill = 0f;
//Let the chainlength be calculated and set it for future access
chainLength = ikChain.calculateChainLength();
//Set the distance which the root joint and the endeffector are allowed to have. If below this distance, stepping is forced.
minDistance = 0.2f * chainLength;
//Set Default Position
defaultPositionLocal = calculateDefault();
lastResortPositionLocal = defaultPositionLocal + new Vector3(0, lastResortHeight * 4f * spider.getNonScaledColliderRadius(), 0);
frontalStartPositionLocal = new Vector3(0, rayFrontalHeight * 4f * spider.getNonScaledColliderRadius(), 0);
// Initialize prediction
prediction = getDefault();
//Set debug variables so they dont draw as lines to the zero point
lastEndEffectorPos = prediction;
projPrediction = prediction;
overshootPrediction = prediction;
// Initialize Casts as either RayCast or SphereCast
casts = new Dictionary <string, Cast>();
updateCasts();
//Set Start Target for IKChain
ikChain.setTarget(getDefaultTarget());
}
// Solves the specific joint for the CCD solver
private static void solveJointCCD(ref JointHinge joint, ref Transform endEffector, ref TargetInfo target, float singularityRadius, bool adjustToTargetNormal)
{
Vector3 rotPoint = joint.getRotationPoint();
Vector3 rotAxis = joint.getRotationAxis();
Vector3 toEnd = Vector3.ProjectOnPlane((endEffector.position - rotPoint), rotAxis);
Vector3 toTarget = Vector3.ProjectOnPlane(target.position - rotPoint, rotAxis);
// If singularity, skip.
if (toTarget == Vector3.zero || toEnd == Vector3.zero)
{
return;
}
if (toTarget.magnitude < singularityRadius)
{
return; // Here even if adjustToTargetNormal is on i might not adjust if target is in this radius.
}
//if (toEnd.magnitude < singularityRadius) return; Notsure if i want this?
float angle;
//This is a special case, where i want the foot, that is the last joint of the chain to adjust to the normal it hit
if (adjustToTargetNormal)
{
angle = footAngleToNormal + 90.0f - Vector3.SignedAngle(Vector3.ProjectOnPlane(target.normal, rotAxis), toEnd, rotAxis);
}
else
{
angle = weight * joint.getWeight() * Vector3.SignedAngle(toEnd, toTarget, rotAxis);
}
joint.applyRotation(angle);
}
public void ConnectFlap(VehiclePartEntity flap, FDSSection flapDat)
{
JointBallSocket jbs = new JointBallSocket(this, flap, flap.GetPosition()); // TODO: necessity?
TheRegion.AddJoint(jbs);
JointNoCollide jnc = new JointNoCollide(this, flap);
TheRegion.AddJoint(jnc);
string mode = flapDat.GetString("mode");
VehicleFlap vf = new VehicleFlap()
{
MaxAngle = flapDat.GetDouble("max_angle", 10).Value,
Speed = flapDat.GetDouble("corrective_speed", 2.25).Value
};
if (mode == "roll/l")
{
JointHinge jh = new JointHinge(this, flap, new Location(1, 0, 0));
TheRegion.AddJoint(jh);
JointVehicleMotor jvm = new JointVehicleMotor(this, flap, new Location(1, 0, 0), true);
TheRegion.AddJoint(jvm);
vf.JVM = jvm;
Flaps_RollL.Add(vf);
}
else if (mode == "roll/r")
{
JointHinge jh = new JointHinge(this, flap, new Location(1, 0, 0));
TheRegion.AddJoint(jh);
JointVehicleMotor jvm = new JointVehicleMotor(this, flap, new Location(1, 0, 0), true);
TheRegion.AddJoint(jvm);
vf.JVM = jvm;
Flaps_RollR.Add(vf);
}
else if (mode == "yaw")
{
JointHinge jh = new JointHinge(this, flap, new Location(0, 0, 1));
TheRegion.AddJoint(jh);
JointVehicleMotor jvm = new JointVehicleMotor(this, flap, new Location(0, 0, 1), true);
TheRegion.AddJoint(jvm);
vf.JVM = jvm;
Flaps_Yaw.Add(vf);
}
else if (mode == "pitch")
{
JointHinge jh = new JointHinge(this, flap, new Location(1, 0, 0));
TheRegion.AddJoint(jh);
JointVehicleMotor jvm = new JointVehicleMotor(this, flap, new Location(1, 0, 0), true);
TheRegion.AddJoint(jvm);
vf.JVM = jvm;
Flaps_Pitch.Add(vf);
}
}
public override bool ParseBytesAndExecute(byte[] data)
{
int len = 1 + 8 + 8 + 8 + 24 + 4 + 4 + 4 + 4 + 24 + 4 + 4 + 4 + 4;
if (data.Length < len)
{
SysConsole.Output(OutputType.WARNING, "Joint packet: Bad initial length!");
return(false);
}
byte type = data[0];
long EID1 = Utilities.BytesToLong(Utilities.BytesPartial(data, 1, 8));
long EID2 = Utilities.BytesToLong(Utilities.BytesPartial(data, 1 + 8, 8));
long JID = Utilities.BytesToLong(Utilities.BytesPartial(data, 1 + 8 + 8, 8));
if (TheClient.TheRegion.GetJoint(JID) != null)
{
// If we already know this joint, just keep on going.
return(true);
}
Entity pe1 = TheClient.TheRegion.GetEntity(EID1);
Entity pe2 = TheClient.TheRegion.GetEntity(EID2);
if (pe1 == null)
{
SysConsole.Output(OutputType.WARNING, "Joint Packet: Invalid EID-1 " + EID1 + " for " + type);
return(false);
}
if (pe2 == null)
{
SysConsole.Output(OutputType.WARNING, "Joint Packet: Invalid EID-2 " + EID2 + " for " + type);
return(false);
}
Location pos1 = Location.FromDoubleBytes(data, 1 + 8 + 8 + 8);
float qx1 = Utilities.BytesToFloat(Utilities.BytesPartial(data, 1 + 8 + 8 + 8 + 24, 4));
float qy1 = Utilities.BytesToFloat(Utilities.BytesPartial(data, 1 + 8 + 8 + 8 + 24 + 4, 4));
float qz1 = Utilities.BytesToFloat(Utilities.BytesPartial(data, 1 + 8 + 8 + 8 + 24 + 4 + 4, 4));
float qw1 = Utilities.BytesToFloat(Utilities.BytesPartial(data, 1 + 8 + 8 + 8 + 24 + 4 + 4 + 4, 4));
BEPUutilities.Quaternion quat1 = new BEPUutilities.Quaternion(qx1, qy1, qz1, qw1);
pe1.SetPosition(pos1);
pe1.SetOrientation(quat1);
Location pos2 = Location.FromDoubleBytes(data, 1 + 8 + 8 + 8 + 24 + 4 + 4 + 4 + 4);
float qx2 = Utilities.BytesToFloat(Utilities.BytesPartial(data, 1 + 8 + 8 + 8 + 24 + 24 + 4 + 4 + 4 + 4, 4));
float qy2 = Utilities.BytesToFloat(Utilities.BytesPartial(data, 1 + 8 + 8 + 8 + 24 + 4 + 24 + 4 + 4 + 4 + 4, 4));
float qz2 = Utilities.BytesToFloat(Utilities.BytesPartial(data, 1 + 8 + 8 + 8 + 24 + 4 + 4 + 24 + 4 + 4 + 4 + 4, 4));
float qw2 = Utilities.BytesToFloat(Utilities.BytesPartial(data, 1 + 8 + 8 + 8 + 24 + 4 + 4 + 4 + 24 + 4 + 4 + 4 + 4, 4));
BEPUutilities.Quaternion quat2 = new BEPUutilities.Quaternion(qx2, qy2, qz2, qw2);
pe1.SetPosition(pos1);
pe1.SetOrientation(quat1);
pe2.SetPosition(pos2);
pe2.SetOrientation(quat2);
// TODO: Registry!
if (type == 0)
{
if (data.Length != len + 24)
{
SysConsole.Output(OutputType.WARNING, "Joint packet: Bad length!");
return(false);
}
Location pos = Location.FromDoubleBytes(data, len);
JointBallSocket jbs = new JointBallSocket((PhysicsEntity)pe1, (PhysicsEntity)pe2, pos)
{
JID = JID
};
TheClient.TheRegion.AddJoint(jbs);
return(true);
}
else if (type == 1)
{
if (data.Length != len + 24)
{
SysConsole.Output(OutputType.WARNING, "Joint packet: Bad length!");
return(false);
}
Location dir = Location.FromDoubleBytes(data, len);
JointSlider js = new JointSlider((PhysicsEntity)pe1, (PhysicsEntity)pe2, dir)
{
JID = JID
};
TheClient.TheRegion.AddJoint(js);
return(true);
}
else if (type == 2)
{
if (data.Length != len + 4 + 4 + 24 + 24)
{
SysConsole.Output(OutputType.WARNING, "Joint packet: Bad length!");
return(false);
}
float min = Utilities.BytesToFloat(Utilities.BytesPartial(data, len, 4));
float max = Utilities.BytesToFloat(Utilities.BytesPartial(data, len + 4, 4));
Location ent1pos = Location.FromDoubleBytes(data, len + 4 + 4);
Location ent2pos = Location.FromDoubleBytes(data, len + 4 + 4 + 24);
JointDistance jd = new JointDistance((PhysicsEntity)pe1, (PhysicsEntity)pe2, min, max, ent1pos, ent2pos)
{
JID = JID
};
TheClient.TheRegion.AddJoint(jd);
return(true);
}
else if (type == 3)
{
if (data.Length != len + 24 + 1)
{
SysConsole.Output(OutputType.WARNING, "Joint packet: Bad length!");
return(false);
}
Location axis = Location.FromDoubleBytes(data, len);
bool mode = data[len + 24] == 1;
JointPullPush jpp = new JointPullPush((PhysicsEntity)pe1, (PhysicsEntity)pe2, axis, mode)
{
JID = JID
};
TheClient.TheRegion.AddJoint(jpp);
return(true);
}
else if (type == 4)
{
if (data.Length != len)
{
SysConsole.Output(OutputType.WARNING, "Joint packet: Bad length!");
return(false);
}
JointForceWeld jfw = new JointForceWeld(pe1, pe2)
{
JID = JID
};
TheClient.TheRegion.AddJoint(jfw);
return(true);
}
else if (type == 5)
{
if (data.Length != len + 24)
{
SysConsole.Output(OutputType.WARNING, "Joint packet: Bad length!");
return(false);
}
Location dir = Location.FromDoubleBytes(data, len);
JointSpinner js = new JointSpinner((PhysicsEntity)pe1, (PhysicsEntity)pe2, dir)
{
JID = JID
};
TheClient.TheRegion.AddJoint(js);
return(true);
}
else if (type == 6)
{
if (data.Length != len + 24 + 24)
{
SysConsole.Output(OutputType.WARNING, "Joint packet: Bad length!");
return(false);
}
Location a1 = Location.FromDoubleBytes(data, len);
Location a2 = Location.FromDoubleBytes(data, len + 24);
JointTwist jt = new JointTwist((PhysicsEntity)pe1, (PhysicsEntity)pe2, a1, a2)
{
JID = JID
};
TheClient.TheRegion.AddJoint(jt);
return(true);
}
else if (type == 7)
{
if (data.Length != len)
{
SysConsole.Output(OutputType.WARNING, "Joint packet: Bad length!");
return(false);
}
JointWeld jw = new JointWeld((PhysicsEntity)pe1, (PhysicsEntity)pe2)
{
JID = JID
};
TheClient.TheRegion.AddJoint(jw);
return(true);
}
else if (type == 8)
{
if (data.Length != len + 24 + 1)
{
SysConsole.Output(OutputType.WARNING, "Joint packet: Bad length!");
return(false);
}
Location dir = Location.FromDoubleBytes(data, len);
bool issteering = data[len + 24] == 1;
JointVehicleMotor jm = new JointVehicleMotor((PhysicsEntity)pe1, (PhysicsEntity)pe2, dir, issteering)
{
JID = JID
};
TheClient.TheRegion.AddJoint(jm);
return(true);
}
else if (type == 9)
{
if (data.Length != len + 24 + 24 + 24 + 4 + 4)
{
SysConsole.Output(OutputType.WARNING, "Joint packet: Bad length!");
return(false);
}
Location cpos1 = Location.FromDoubleBytes(data, len);
Location cpos2 = Location.FromDoubleBytes(data, len + 24);
Location axis = Location.FromDoubleBytes(data, len + 24 + 24);
float min = Utilities.BytesToFloat(Utilities.BytesPartial(data, len + 24 + 24 + 24, 4));
float max = Utilities.BytesToFloat(Utilities.BytesPartial(data, len + 24 + 24 + 24 + 4, 4));
JointLAxisLimit jlal = new JointLAxisLimit((PhysicsEntity)pe1, (PhysicsEntity)pe2, min, max, cpos1, cpos2, axis)
{
JID = JID
};
TheClient.TheRegion.AddJoint(jlal);
return(true);
}
else if (type == 10)
{
if (data.Length != len + 24 + 24)
{
SysConsole.Output(OutputType.WARNING, "Joint packet: Bad length!");
return(false);
}
Location hinge = Location.FromDoubleBytes(data, len);
Location twist = Location.FromDoubleBytes(data, len + 24);
JointSwivelHinge jlal = new JointSwivelHinge((PhysicsEntity)pe1, (PhysicsEntity)pe2, hinge, twist)
{
JID = JID
};
TheClient.TheRegion.AddJoint(jlal);
return(true);
}
else if (type == 11)
{
if (data.Length != len + 4)
{
SysConsole.Output(OutputType.WARNING, "Joint packet: Bad length!");
return(false);
}
float height = Utilities.BytesToFloat(Utilities.BytesPartial(data, len, 4));
ConstWheelStepUp cwsu = new ConstWheelStepUp((PhysicsEntity)pe1, height)
{
JID = JID
};
TheClient.TheRegion.AddJoint(cwsu);
return(true);
}
else if (type == 12)
{
if (data.Length != len + 4 * 4 + 1)
{
SysConsole.Output(OutputType.WARNING, "Joint packet: Bad length!");
return(false);
}
ConnectorBeam cb = new ConnectorBeam()
{
One = pe1,
Two = pe2,
color = new Color4F(Utilities.BytesToFloat(Utilities.BytesPartial(data, len, 4)), Utilities.BytesToFloat(Utilities.BytesPartial(data, len + 4, 4)), Utilities.BytesToFloat(Utilities.BytesPartial(data, len + 4 * 2, 4)), Utilities.BytesToFloat(Utilities.BytesPartial(data, len + 4 * 3, 4))),
JID = JID,
type = (BeamType)data[len + 4]
};
TheClient.TheRegion.AddJoint(cb);
return(true);
}
else if (type == 13)
{
if (data.Length != len)
{
SysConsole.Output(OutputType.WARNING, "Joint packet: Bad length!");
return(false);
}
JointFlyingDisc jfd = new JointFlyingDisc((PhysicsEntity)pe1)
{
JID = JID
};
TheClient.TheRegion.AddJoint(jfd);
return(true);
}
else if (type == 14)
{
if (data.Length != len)
{
SysConsole.Output(OutputType.WARNING, "Joint packet: Bad length!");
return(false);
}
JointNoCollide jnc = new JointNoCollide((PhysicsEntity)pe1, (PhysicsEntity)pe2)
{
JID = JID
};
TheClient.TheRegion.AddJoint(jnc);
return(true);
}
else if (type == 15)
{
if (data.Length != len + 24)
{
SysConsole.Output(OutputType.WARNING, "Joint packet: Bad length!");
return(false);
}
Location hinge = Location.FromDoubleBytes(data, len);
JointHinge jlal = new JointHinge((PhysicsEntity)pe1, (PhysicsEntity)pe2, hinge)
{
JID = JID
};
TheClient.TheRegion.AddJoint(jlal);
return(true);
}
else
{
SysConsole.Output(OutputType.WARNING, "Unknown joint type " + type);
return(false);
}
}
private void addJoined1_AddJoint(object sender, AddJoinedBodiesArgs e)
{
try
{
// Figure out the centerpoint
Point3D centerPoint = Math3D.GetRandomVector_Spherical(CREATEOBJECTBOUNDRY).ToPoint();
// Get a random rotation
//TODO: Figure out why it fails when I give it a rotation
//Quaternion finalRotation = new Quaternion(Math3D.GetRandomVectorSpherical(_rand, 10), Math3D.GetNearZeroValue(_rand, 360d));
Quaternion finalRotation = new Quaternion(new Vector3D(0, 0, 1), 0);
// All bodies will be the same color
Color color = UtilityWPF.GetRandomColor(64, 192);
Body[] bodies = null;
#region Get Bodies
switch (e.JointType)
{
case AddJointType.BallAndSocket:
case AddJointType.Hinge:
case AddJointType.Slider:
case AddJointType.Corkscrew:
case AddJointType.UniversalJoint:
bodies = new Body[2];
GetJointBodyPair(out bodies[0], out bodies[1], e.Body1Type, e.Body2Type, centerPoint, finalRotation, e.SeparationDistance, color);
break;
case AddJointType.UpVector:
case AddJointType.Multi_BallAndChain:
case AddJointType.Multi_Tetrahedron:
MessageBox.Show("finish this");
return;
default:
throw new ApplicationException("Unknown AddJointType: " + e.JointType.ToString());
}
#endregion
_bodySets.Add(bodies);
#region Setup Joint
Vector3D directionAlong = finalRotation.GetRotatedVector(new Vector3D(1, 0, 0));
Vector3D directionOrth1 = finalRotation.GetRotatedVector(new Vector3D(0, 1, 0));
Vector3D directionOrth2 = finalRotation.GetRotatedVector(new Vector3D(0, 0, 1));
switch (e.JointType)
{
case AddJointType.BallAndSocket:
#region BallAndSocket
JointBallAndSocket ballAndSocket = JointBallAndSocket.CreateBallAndSocket(_world, centerPoint, bodies[0], bodies[1]);
ballAndSocket.ShouldLinkedBodiesCollideEachOther = true;
//TODO: Let the user define these limits
//ballAndSocket.SetConeLimits();
#endregion
break;
case AddJointType.Hinge:
#region Hinge
JointHinge hinge = JointHinge.CreateHinge(_world, centerPoint, directionOrth1, bodies[0], bodies[1]);
hinge.ShouldLinkedBodiesCollideEachOther = true;
#endregion
break;
case AddJointType.Slider:
#region Slider
JointSlider slider = JointSlider.CreateSlider(_world, centerPoint, directionAlong, bodies[0], bodies[1]);
slider.ShouldLinkedBodiesCollideEachOther = true;
#endregion
break;
case AddJointType.Corkscrew:
#region Corkscrew
JointCorkscrew corkscrew = JointCorkscrew.CreateCorkscrew(_world, centerPoint, directionAlong, bodies[0], bodies[1]);
corkscrew.ShouldLinkedBodiesCollideEachOther = true;
#endregion
break;
case AddJointType.UniversalJoint:
#region UniversalJoint
JointUniversal uJoint = JointUniversal.CreateUniversal(_world, centerPoint, directionOrth1, directionOrth2, bodies[0], bodies[1]);
uJoint.ShouldLinkedBodiesCollideEachOther = true;
#endregion
break;
default:
throw new ApplicationException("Unexpected AddJointType: " + e.JointType.ToString());
}
#endregion
BodiesAdded(bodies);
}
catch (Exception ex)
{
MessageBox.Show(ex.ToString(), this.Title, MessageBoxButton.OK, MessageBoxImage.Error);
}
}
