private void CreateSlider()
{
float mass = 1.0f;
RigidBody bodyA = LocalCreateRigidBody(mass, Matrix.Translation(-20, 0, 15), cubeShape);
bodyA.ActivationState = ActivationState.DisableDeactivation;
// add dynamic rigid body B1
RigidBody bodyB = LocalCreateRigidBody(0, Matrix.Translation(-30, 0, 15), cubeShape);
//RigidBody bodyB = LocalCreateRigidBody(mass, Matrix.Translation(-20, 0, 15), cubeShape);
bodyB.ActivationState = ActivationState.DisableDeactivation;
// create slider constraint between A1 and B1 and add it to world
var slider = new SliderConstraint(bodyA, bodyB, Matrix.Identity, Matrix.Identity, true)
{
LowerLinearLimit = -15.0f,
UpperLinearLimit = -5.0f,
//LowerLinearLimit = -10.0f,
//UpperLinearLimit = -10.0f,
LowerAngularLimit = -(float)Math.PI / 3.0f,
UpperAngularLimit = (float)Math.PI / 3.0f,
DebugDrawSize = 5.0f
};
World.AddConstraint(slider, true);
}
public void Evaluate(int SpreadMax)
{
IConstraintContainer inputWorld = this.contraintContainer[0];
if (inputWorld != null)
{
this.persister.UpdateWorld(inputWorld);
for (int i = 0; i < SpreadMax; i++)
{
if (FCreate[i])
{
RigidBody body = FBody[i];
if (body != null)
{
SlimDX.Matrix m = this.FFrame[i];
SliderConstraint cst = new SliderConstraint(body, *(BulletSharp.Matrix *) & m, true);
cst.LowerLinLimit = this.FLinearLimit[i].X;
cst.UpperLinLimit = this.FLinearLimit[i].Y;
cst.LowerAngularLimit = this.FAngularLimit[i].X * (float)Math.PI * 2.0f;
cst.UpperAngularLimit = this.FAngularLimit[i].Y * (float)Math.PI * 2.0f;
cst.DebugDrawSize = 5.0f;
this.persister.Append(cst);
}
}
}
this.persister.Flush();
}
else
{
this.constraintsOutput.SliceCount = 0;
}
}
//called by Physics World just before constraint is added to world.
//the current constraint properties are used to rebuild the constraint.
internal override bool _BuildConstraint()
{
BPhysicsWorld world = BPhysicsWorld.Get();
if (constraintPtr != null)
{
if (isInWorld && world != null)
{
isInWorld = false;
world.RemoveConstraint(constraintPtr);
}
}
if (targetRigidBodyA == null)
{
Debug.LogError("Constraint target rigid body was not set.");
return(false);
}
RigidBody rba = (RigidBody)targetRigidBodyA.GetCollisionObject();
if (rba == null)
{
Debug.LogError("Constraint could not get bullet RigidBody from target rigid body");
return(false);
}
if (constraintType == ConstraintType.constrainToAnotherBody)
{
RigidBody rbb = (RigidBody)targetRigidBodyB.GetCollisionObject();
if (rbb == null)
{
Debug.LogError("Constraint could not get bullet RigidBody from target rigid body");
return(false);
}
BulletSharp.Math.Matrix frameInA = BulletSharp.Math.Matrix.AffineTransformation(1f, Quaternion.LookRotation(localForwardInA, localUpInA).ToBullet(), localPointInA.ToBullet());
BulletSharp.Math.Matrix frameInB = BulletSharp.Math.Matrix.AffineTransformation(1f, Quaternion.LookRotation(localForwardInB, localUpInB).ToBullet(), localPointInB.ToBullet());
constraintPtr = new SliderConstraint(rba, rbb, frameInA, frameInB, false);
constraintPtr.Userobject = this;
}
else
{
BulletSharp.Math.Matrix frameInA = BulletSharp.Math.Matrix.AffineTransformation(1f, Quaternion.LookRotation(localForwardInA, localUpInA).ToBullet(), localPointInA.ToBullet());
constraintPtr = new SliderConstraint(rba, frameInA, false);
constraintPtr.Userobject = this;
}
SliderConstraint sl = (SliderConstraint)constraintPtr;
sl.LowerLinearLimit = lowerLinearLimit;
sl.UpperLinearLimit = upperLinearLimit;
sl.LowerAngularLimit = lowerAngularLimit;
sl.UpperAngularLimit = upperAngularLimit;
constraintPtr.Userobject = this;
return(true);
}
public ISliderConstraintImp AddSliderConstraint(IRigidBodyImp rigidBodyA, float4x4 frameInA, bool useLinearReferenceFrameA)
{
var rigidBodyAImp = (RigidBodyImp)rigidBodyA;
var btRigidBodyA = rigidBodyAImp._rbi;
var btFrameInA = Translater.Float4X4ToBtMatrix(frameInA);
var btSliderConstraint = new SliderConstraint(btRigidBodyA, btFrameInA, useLinearReferenceFrameA);
BtWorld.AddConstraint(btSliderConstraint);
var retval = new SliderConstraintImp();
retval._sci = btSliderConstraint;
btSliderConstraint.UserObject = retval;
return(retval);
}
public SliderJoint(
ICollisionShape shapeA,
ICollisionShape shapeB,
Vector3d startAnchorPosition,
Vector3d sliderAxis,
double restoreCoefficient,
double springCoefficient)
{
sliderConstraint = new SliderConstraint(
((IMapper)shapeA).GetShape(),
((IMapper)shapeB).GetShape(),
startAnchorPosition,
sliderAxis,
restoreCoefficient,
springCoefficient);
}
//called by Physics World just before constraint is added to world.
//the current constraint properties are used to rebuild the constraint.
internal override bool _BuildConstraint()
{
BPhysicsWorld world = BPhysicsWorld.Get();
if (m_constraintPtr != null)
{
if (m_isInWorld && world != null)
{
m_isInWorld = false;
world.RemoveConstraint(m_constraintPtr);
}
}
BRigidBody targetRigidBodyA = GetComponent <BRigidBody>();
if (targetRigidBodyA == null)
{
Debug.LogError("BSliderConstraint needs to be added to a component with a BRigidBody.");
return(false);
}
if (!targetRigidBodyA.isInWorld)
{
world.AddRigidBody(targetRigidBodyA);
}
RigidBody rba = (RigidBody)targetRigidBodyA.GetCollisionObject();
if (rba == null)
{
Debug.LogError("Constraint could not get bullet RigidBody from target rigid body");
return(false);
}
if (m_constraintType == ConstraintType.constrainToAnotherBody)
{
if (m_otherRigidBody == null)
{
Debug.LogError("Other rigid body was not set");
return(false);
}
if (!m_otherRigidBody.isInWorld)
{
world.AddRigidBody(m_otherRigidBody);
}
RigidBody rbb = (RigidBody)m_otherRigidBody.GetCollisionObject();
if (rbb == null)
{
Debug.LogError("Constraint could not get bullet RigidBody from target rigid body");
return(false);
}
BM.Matrix frameInA, frameInOther;
string errormsg = "";
if (CreateFramesA_B(m_localConstraintAxisX, m_localConstraintAxisY, m_localConstraintPoint, out frameInA, out frameInOther, ref errormsg))
{
m_constraintPtr = new SliderConstraint(rbb, rba, frameInOther, frameInA, true);
}
else
{
Debug.LogError(errormsg);
return(false);
}
}
else
{
BulletSharp.Math.Matrix frameInA = BM.Matrix.Identity;
string errormsg = "";
if (CreateFrame(m_localConstraintAxisX, m_localConstraintAxisY, m_localConstraintPoint, ref frameInA, ref errormsg))
{
m_constraintPtr = new SliderConstraint(rba, frameInA, true);
}
else
{
Debug.LogError(errormsg);
return(false);
}
}
SliderConstraint sl = (SliderConstraint)m_constraintPtr;
sl.LowerLinearLimit = m_lowerLinearLimit;
sl.UpperLinearLimit = m_upperLinearLimit;
sl.LowerAngularLimit = m_lowerAngularLimitRadians;
sl.UpperAngularLimit = m_upperAngularLimitRadians;
m_constraintPtr.Userobject = this;
m_constraintPtr.DebugDrawSize = m_debugDrawSize;
m_constraintPtr.BreakingImpulseThreshold = m_breakingImpulseThreshold;
return(true);
}
public static void UpdateAllMotors(RigidNode_Base skeleton, UnityPacket.OutputStatePacket.DIOModule[] dioModules)
{
float[] pwm = dioModules[0].pwmValues;
float[] can = dioModules[0].canValues;
if (Input.anyKey)
{
pwm[0] +=
(Input.GetKey(KeyCode.UpArrow) ? SPEED_ARROW_PWM : 0.0f) +
(Input.GetKey(KeyCode.DownArrow) ? -SPEED_ARROW_PWM : 0.0f) +
(Input.GetKey(KeyCode.LeftArrow) ? SPEED_ARROW_PWM : 0.0f) +
(Input.GetKey(KeyCode.RightArrow) ? -SPEED_ARROW_PWM : 0.0f);
pwm[1] +=
(Input.GetKey(KeyCode.UpArrow) ? -SPEED_ARROW_PWM : 0.0f) +
(Input.GetKey(KeyCode.DownArrow) ? SPEED_ARROW_PWM : 0.0f) +
(Input.GetKey(KeyCode.LeftArrow) ? SPEED_ARROW_PWM : 0.0f) +
(Input.GetKey(KeyCode.RightArrow) ? -SPEED_ARROW_PWM : 0.0f);
pwm[2] += Input.GetKey(KeyCode.Alpha1) ? SPEED_ARROW_PWM : Input.GetKey(KeyCode.Alpha2) ? -SPEED_ARROW_PWM : 0f;
pwm[3] += Input.GetKey(KeyCode.Alpha3) ? SPEED_ARROW_PWM : Input.GetKey(KeyCode.Alpha4) ? -SPEED_ARROW_PWM : 0f;
pwm[4] += Input.GetKey(KeyCode.Alpha5) ? SPEED_ARROW_PWM : Input.GetKey(KeyCode.Alpha6) ? -SPEED_ARROW_PWM : 0f;
pwm[5] += Input.GetKey(KeyCode.Alpha7) ? SPEED_ARROW_PWM : Input.GetKey(KeyCode.Alpha8) ? -SPEED_ARROW_PWM : 0f;
pwm[6] += Input.GetKey(KeyCode.Alpha9) ? SPEED_ARROW_PWM : Input.GetKey(KeyCode.Alpha0) ? -SPEED_ARROW_PWM : 0f;
}
List <RigidNode_Base> listOfSubNodes = new List <RigidNode_Base>();
skeleton.ListAllNodes(listOfSubNodes);
for (int i = 0; i < pwm.Length; i++)
{
foreach (RigidNode_Base node in listOfSubNodes)
{
RigidNode rigidNode = (RigidNode)node;
if (rigidNode.GetSkeletalJoint() != null && rigidNode.GetSkeletalJoint().cDriver != null)
{
if (rigidNode.GetSkeletalJoint().cDriver.GetDriveType().IsMotor())
{
if (rigidNode.GetSkeletalJoint().cDriver.portA == i + 1)
{
float maxSpeed = 0f;
float impulse = 0f;
float friction = 0f;
if (rigidNode.HasDriverMeta <WheelDriverMeta>())
{
maxSpeed = WHEEL_MAX_SPEED;
impulse = WHEEL_MOTOR_IMPULSE;
friction = WHEEL_COAST_FRICTION;
}
else
{
maxSpeed = HINGE_MAX_SPEED;
impulse = HINGE_MOTOR_IMPULSE;
friction = HINGE_COAST_FRICTION;
}
BHingedConstraint hingedConstraint = rigidNode.MainObject.GetComponent <BHingedConstraint>();
hingedConstraint.enableMotor = true;
hingedConstraint.targetMotorAngularVelocity = pwm[i] > 0f ? maxSpeed : pwm[i] < 0f ? -maxSpeed : 0f;
hingedConstraint.maxMotorImpulse = pwm[i] == 0f ? friction : Mathf.Abs(pwm[i] * impulse);
}
}
else if (rigidNode.GetSkeletalJoint().cDriver.GetDriveType().IsElevator())
{
if (rigidNode.GetSkeletalJoint().cDriver.portA == i + 1 && rigidNode.HasDriverMeta <ElevatorDriverMeta>())
{
BSliderConstraint bSliderConstraint = rigidNode.MainObject.GetComponent <BSliderConstraint>();
SliderConstraint sc = (SliderConstraint)bSliderConstraint.GetConstraint();
sc.PoweredLinearMotor = true;
sc.MaxLinearMotorForce = MAX_SLIDER_FORCE;
sc.TargetLinearMotorVelocity = pwm[i] * MAX_SLIDER_SPEED;
}
}
}
}
}
}
/// <summary>
/// Updates all motor values from the given <see cref="RigidNode_Base"/>, pwm values, and emulation network info.
/// </summary>
/// <param name="skeleton"></param>
/// <param name="pwm"></param>
/// <param name="emuList"></param>
public static void UpdateAllMotors(RigidNode_Base skeleton, float[] pwm, List <Synthesis.Robot.RobotBase.EmuNetworkInfo> emuList)
{
listOfSubNodes.Clear();
skeleton.ListAllNodes(listOfSubNodes);
UpdateEmulationJoysticks();
UpdateEmulationMotors(pwm);
UpdateEmulationSensors(emuList);
foreach (RigidNode node in listOfSubNodes.Select(n => n as RigidNode))
{
SkeletalJoint_Base joint = node.GetSkeletalJoint();
if (joint == null || joint.cDriver == null)
{
continue;
}
BRaycastWheel raycastWheel = node.MainObject.GetComponent <BRaycastWheel>();
if (raycastWheel != null)
{
float output = motors[node.GetSkeletalJoint().cDriver.port1 - 1];
if (node.GetSkeletalJoint().cDriver.InputGear != 0 && node.GetSkeletalJoint().cDriver.OutputGear != 0)
{
output *= Convert.ToSingle(node.GetSkeletalJoint().cDriver.InputGear / node.GetSkeletalJoint().cDriver.OutputGear);
}
raycastWheel.ApplyForce(output);
}
else if (joint.cDriver.GetDriveType().IsMotor() && node.MainObject.GetComponent <BHingedConstraint>() != null)
{
if (!joint.cDriver.isCan)
{
float maxSpeed = 0f;
float impulse = 0f;
float friction = 0f;
float output = motors[joint.cDriver.port1 - 1];
if (joint.cDriver.InputGear != 0 && joint.cDriver.OutputGear != 0)
{
impulse *= Convert.ToSingle(joint.cDriver.InputGear / joint.cDriver.OutputGear);
}
if (node.HasDriverMeta <WheelDriverMeta>())
{
maxSpeed = WheelMaxSpeed;
impulse = WheelMotorImpulse;
friction = WheelCoastFriction;
}
else
{
maxSpeed = HingeMaxSpeed;
impulse = HingeMotorImpulse;
friction = HingeCostFriction;
}
BHingedConstraint hingedConstraint = node.MainObject.GetComponent <BHingedConstraint>();
hingedConstraint.enableMotor = true;
hingedConstraint.targetMotorAngularVelocity = output > 0f ? maxSpeed : output < 0f ? -maxSpeed : 0f;
hingedConstraint.maxMotorImpulse = node.GetSkeletalJoint().cDriver.hasBrake ? HingeMotorImpulse : output == 0f ? friction : Mathf.Abs(output * impulse);
}
else
{
float maxSpeed = 0f;
float impulse = 0f;
float friction = 0f;
float output = motors[joint.cDriver.port1 - 10];
if (joint.cDriver.InputGear != 0 && joint.cDriver.OutputGear != 0)
{
impulse *= Convert.ToSingle(joint.cDriver.InputGear / joint.cDriver.OutputGear);
}
if (node.HasDriverMeta <WheelDriverMeta>())
{
maxSpeed = WheelMaxSpeed;
impulse = WheelMotorImpulse;
friction = WheelCoastFriction;
}
else
{
maxSpeed = HingeMaxSpeed;
impulse = HingeMotorImpulse;
friction = HingeCostFriction;
}
BHingedConstraint hingedConstraint = node.MainObject.GetComponent <BHingedConstraint>();
hingedConstraint.enableMotor = true;
hingedConstraint.targetMotorAngularVelocity = output > 0f ? maxSpeed : output < 0f ? -maxSpeed : 0f;
hingedConstraint.maxMotorImpulse = node.GetSkeletalJoint().cDriver.hasBrake ? HingeMotorImpulse : output == 0f ? friction : Mathf.Abs(output * impulse);
}
}
else if (joint.cDriver.GetDriveType().IsElevator() && node.HasDriverMeta <ElevatorDriverMeta>())
{
float output = motors[joint.cDriver.port1 - 1];
BSliderConstraint bSliderConstraint = node.MainObject.GetComponent <BSliderConstraint>();
SliderConstraint sc = (SliderConstraint)bSliderConstraint.GetConstraint();
sc.PoweredLinearMotor = true;
sc.MaxLinearMotorForce = MaxSliderForce;
sc.TargetLinearMotorVelocity = output * MaxSliderSpeed;
}
}
}
/// <summary>
/// Updates the motors on the manipulator in mix and match mode. Called every frame.
/// </summary>
/// <param name="skeleton"></param>
/// <param name="dioModules"></param>
/// <param name="controlIndex"></param>
public static void UpdateManipulatorMotors(RigidNode_Base skeleton, int controlIndex, List <Synthesis.Robot.RobotBase.EmuNetworkInfo> emuList)
{
UpdateAllOutputs(controlIndex, emuList);
listOfSubNodes.Clear();
skeleton.ListAllNodes(listOfSubNodes);
foreach (RigidNode_Base node in listOfSubNodes)
{
RigidNode rigidNode = (RigidNode)node;
BRaycastWheel raycastWheel = rigidNode.MainObject.GetComponent <BRaycastWheel>();
if (raycastWheel != null)
{
float force = GetOutput(rigidNode.GetSkeletalJoint().cDriver);
if (rigidNode.GetSkeletalJoint().cDriver.InputGear != 0 && rigidNode.GetSkeletalJoint().cDriver.OutputGear != 0)
{
force *= Convert.ToSingle(rigidNode.GetSkeletalJoint().cDriver.InputGear / rigidNode.GetSkeletalJoint().cDriver.OutputGear);
}
raycastWheel.ApplyForce(force);
}
if (rigidNode.GetSkeletalJoint() != null && rigidNode.GetSkeletalJoint().cDriver != null)
{
if (rigidNode.GetSkeletalJoint().cDriver.GetDriveType().IsMotor() && rigidNode.MainObject.GetComponent <BHingedConstraint>() != null)
{
float output = GetOutput(rigidNode.GetSkeletalJoint().cDriver);
float maxSpeed = 0f;
float impulse = 0f;
float friction = 0f;
if (rigidNode.GetSkeletalJoint().cDriver.InputGear != 0 && rigidNode.GetSkeletalJoint().cDriver.OutputGear != 0)
{
impulse *= Convert.ToSingle(rigidNode.GetSkeletalJoint().cDriver.InputGear / rigidNode.GetSkeletalJoint().cDriver.OutputGear);
}
if (rigidNode.HasDriverMeta <WheelDriverMeta>())
{
maxSpeed = WheelMaxSpeed;
impulse = WheelMotorImpulse;
friction = WheelCoastFriction;
}
else
{
maxSpeed = HingeMaxSpeed;
impulse = HingeMotorImpulse;
friction = HingeCostFriction;
}
BHingedConstraint hingedConstraint = rigidNode.MainObject.GetComponent <BHingedConstraint>();
hingedConstraint.enableMotor = true;
hingedConstraint.targetMotorAngularVelocity = output > 0f ? maxSpeed : output < 0f ? -maxSpeed : 0f;
hingedConstraint.maxMotorImpulse = rigidNode.GetSkeletalJoint().cDriver.hasBrake ? HingeMotorImpulse : output == 0f ? friction : Mathf.Abs(output * impulse);
}
else if (rigidNode.GetSkeletalJoint().cDriver.GetDriveType().IsElevator())
{
if (rigidNode.HasDriverMeta <ElevatorDriverMeta>())
{
float output = GetOutput(rigidNode.GetSkeletalJoint().cDriver);
BSliderConstraint bSliderConstraint = rigidNode.MainObject.GetComponent <BSliderConstraint>();
SliderConstraint sc = (SliderConstraint)bSliderConstraint.GetConstraint();
sc.PoweredLinearMotor = true;
sc.MaxLinearMotorForce = MaxSliderForce;
sc.TargetLinearMotorVelocity = output * MaxSliderSpeed;
}
}
}
}
}
public static void UpdateAllMotors(RigidNode_Base skeleton, UnityPacket.OutputStatePacket.DIOModule[] dioModules, int controlIndex, bool mecanum)
{
bool IsMecanum = mecanum;
int reverse = -1;
float[] pwm = new float[10];
float[] can = new float[10];
if (dioModules[0] != null)
{
pwm = dioModules[0].pwmValues;
can = dioModules[0].canValues;
}
if (IsMecanum)
{
pwm[(int)MecanumPorts.FRONT_RIGHT] +=
(InputControl.GetButton(Controls.buttons[controlIndex].forward) ? reverse * SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].backward) ? reverse * -SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].left) ? reverse * -SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].right) ? reverse * SPEED_ARROW_PWM : 0.0f) +
(Input.GetKey(KeyCode.O) ? reverse * SPEED_ARROW_PWM : 0.0f) + //Left Rotate
(Input.GetKey(KeyCode.P) ? reverse * -SPEED_ARROW_PWM : 0.0f); //Right Rotate
pwm[(int)MecanumPorts.BACK_LEFT] +=
(InputControl.GetButton(Controls.buttons[controlIndex].forward) ? SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].backward) ? -SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].left) ? -SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].right) ? SPEED_ARROW_PWM : 0.0f) +
(Input.GetKey(KeyCode.O) ? -SPEED_ARROW_PWM : 0.0f) + //Left Rotate
(Input.GetKey(KeyCode.P) ? SPEED_ARROW_PWM : 0.0f); //Right Rotate
pwm[(int)MecanumPorts.FRONT_LEFT] +=
(InputControl.GetButton(Controls.buttons[controlIndex].forward) ? SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].backward) ? -SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].left) ? SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].right) ? -SPEED_ARROW_PWM : 0.0f) +
(Input.GetKey(KeyCode.O) ? -SPEED_ARROW_PWM : 0.0f) + //Left Rotate
(Input.GetKey(KeyCode.P) ? SPEED_ARROW_PWM : 0.0f); //Right Rotate
pwm[(int)MecanumPorts.BACK_RIGHT] +=
(InputControl.GetButton(Controls.buttons[controlIndex].forward) ? reverse * SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].backward) ? reverse * -SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].left) ? reverse * SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].right) ? reverse * -SPEED_ARROW_PWM : 0.0f) +
(Input.GetKey(KeyCode.O) ? reverse * SPEED_ARROW_PWM : 0.0f) + //Left Rotate
(Input.GetKey(KeyCode.P) ? reverse * -SPEED_ARROW_PWM : 0.0f); //Right Rotate
}
else
{
pwm[0] +=
(InputControl.GetButton(Controls.buttons[controlIndex].forward) ? -SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].backward) ? SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].left) ? -SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].right) ? SPEED_ARROW_PWM : 0.0f);
pwm[1] +=
(InputControl.GetButton(Controls.buttons[controlIndex].forward) ? SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].backward) ? -SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].left) ? -SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].right) ? SPEED_ARROW_PWM : 0.0f);
pwm[2] +=
(InputControl.GetButton(Controls.buttons[controlIndex].pwm2Plus)) ? SPEED_ARROW_PWM :
(InputControl.GetButton(Controls.buttons[controlIndex].pwm2Neg)) ? -SPEED_ARROW_PWM : 0f;
pwm[3] +=
(InputControl.GetButton(Controls.buttons[controlIndex].pwm3Plus)) ? SPEED_ARROW_PWM :
(InputControl.GetButton(Controls.buttons[controlIndex].pwm3Neg)) ? -SPEED_ARROW_PWM : 0f;
pwm[4] +=
(InputControl.GetButton(Controls.buttons[controlIndex].pwm4Plus)) ? SPEED_ARROW_PWM :
(InputControl.GetButton(Controls.buttons[controlIndex].pwm4Neg)) ? -SPEED_ARROW_PWM : 0f;
pwm[5] +=
(InputControl.GetButton(Controls.buttons[controlIndex].pwm5Plus)) ? SPEED_ARROW_PWM :
(InputControl.GetButton(Controls.buttons[controlIndex].pwm5Neg)) ? -SPEED_ARROW_PWM : 0f;
pwm[6] +=
(InputControl.GetButton(Controls.buttons[controlIndex].pwm6Plus)) ? SPEED_ARROW_PWM :
(InputControl.GetButton(Controls.buttons[controlIndex].pwm6Neg)) ? -SPEED_ARROW_PWM : 0f;
}
List <RigidNode_Base> listOfSubNodes = new List <RigidNode_Base>();
skeleton.ListAllNodes(listOfSubNodes);
for (int i = 0; i < pwm.Length; i++)
{
foreach (RigidNode_Base node in listOfSubNodes)
{
RigidNode rigidNode = (RigidNode)node;
if (rigidNode.GetSkeletalJoint() != null && rigidNode.GetSkeletalJoint().cDriver != null)
{
if (rigidNode.GetSkeletalJoint().cDriver.GetDriveType().IsMotor())
{
if (rigidNode.GetSkeletalJoint().cDriver.portA == i + 1)
{
float maxSpeed = 0f;
float impulse = 0f;
float friction = 0f;
if (rigidNode.HasDriverMeta <WheelDriverMeta>())
{
maxSpeed = WHEEL_MAX_SPEED;
impulse = WHEEL_MOTOR_IMPULSE;
friction = WHEEL_COAST_FRICTION;
}
else
{
maxSpeed = HINGE_MAX_SPEED;
impulse = HINGE_MOTOR_IMPULSE;
friction = HINGE_COAST_FRICTION;
}
BHingedConstraint hingedConstraint = rigidNode.MainObject.GetComponent <BHingedConstraint>();
hingedConstraint.enableMotor = true;
hingedConstraint.targetMotorAngularVelocity = pwm[i] > 0f ? maxSpeed : pwm[i] < 0f ? -maxSpeed : 0f;
hingedConstraint.maxMotorImpulse = pwm[i] == 0f ? friction : Mathf.Abs(pwm[i] * impulse);
}
}
else if (rigidNode.GetSkeletalJoint().cDriver.GetDriveType().IsElevator())
{
if (rigidNode.GetSkeletalJoint().cDriver.portA == i + 1 && rigidNode.HasDriverMeta <ElevatorDriverMeta>())
{
BSliderConstraint bSliderConstraint = rigidNode.MainObject.GetComponent <BSliderConstraint>();
SliderConstraint sc = (SliderConstraint)bSliderConstraint.GetConstraint();
sc.PoweredLinearMotor = true;
sc.MaxLinearMotorForce = MAX_SLIDER_FORCE;
sc.TargetLinearMotorVelocity = pwm[i] * MAX_SLIDER_SPEED;
}
}
}
}
}
}
public ISliderConstraintImp AddSliderConstraint(IRigidBodyImp rigidBodyA, float4x4 frameInA, bool useLinearReferenceFrameA)
{
var rigidBodyAImp = (RigidBodyImp)rigidBodyA;
var btRigidBodyA = rigidBodyAImp._rbi;
var btFrameInA = Translater.Float4X4ToBtMatrix(frameInA);
var btSliderConstraint = new SliderConstraint(btRigidBodyA, btFrameInA, useLinearReferenceFrameA);
BtWorld.AddConstraint(btSliderConstraint);
var retval = new SliderConstraintImp();
retval._sci = btSliderConstraint;
btSliderConstraint.UserObject = retval;
return retval;
}
/// <summary>
/// Updates the motors on the manipulator in mix and match mode. Called every frame.
/// </summary>
/// <param name="skeleton"></param>
/// <param name="dioModules"></param>
/// <param name="controlIndex"></param>
public static void UpdateManipulatorMotors(RigidNode_Base skeleton, UnityPacket.OutputStatePacket.DIOModule[] dioModules, int controlIndex)
{
float[] pwm;
float[] can;
if (dioModules[0] != null)
{
pwm = dioModules[0].pwmValues;
can = dioModules[0].canValues;
}
else
{
pwm = new float[10];
can = new float[10];
}
pwm[4] +=
(InputControl.GetAxis(Controls.axes[controlIndex].pwm4Axes) * SPEED_ARROW_PWM);
pwm[5] +=
(InputControl.GetAxis(Controls.axes[controlIndex].pwm5Axes) * SPEED_ARROW_PWM);
pwm[6] +=
(InputControl.GetAxis(Controls.axes[controlIndex].pwm6Axes) * SPEED_ARROW_PWM);
listOfSubNodes.Clear();
skeleton.ListAllNodes(listOfSubNodes);
for (int i = 0; i < pwm.Length; i++)
{
foreach (RigidNode_Base node in listOfSubNodes)
{
RigidNode rigidNode = (RigidNode)node;
BRaycastWheel raycastWheel = rigidNode.MainObject.GetComponent <BRaycastWheel>();
if (raycastWheel != null)
{
if (rigidNode.GetSkeletalJoint().cDriver.portA == i + 1)
{
raycastWheel.ApplyForce(pwm[i]);
}
}
if (rigidNode.GetSkeletalJoint() != null && rigidNode.GetSkeletalJoint().cDriver != null)
{
if (rigidNode.GetSkeletalJoint().cDriver.GetDriveType().IsMotor() && rigidNode.MainObject.GetComponent <BHingedConstraint>() != null)
{
if (rigidNode.GetSkeletalJoint().cDriver.portA == i + 1)
{
float maxSpeed = 0f;
float impulse = 0f;
float friction = 0f;
if (rigidNode.HasDriverMeta <WheelDriverMeta>())
{
maxSpeed = WHEEL_MAX_SPEED;
impulse = WHEEL_MOTOR_IMPULSE;
friction = WHEEL_COAST_FRICTION;
}
else
{
maxSpeed = HINGE_MAX_SPEED;
impulse = HINGE_MOTOR_IMPULSE;
friction = HINGE_COAST_FRICTION;
}
BHingedConstraint hingedConstraint = rigidNode.MainObject.GetComponent <BHingedConstraint>();
hingedConstraint.enableMotor = true;
hingedConstraint.targetMotorAngularVelocity = pwm[i] > 0f ? maxSpeed : pwm[i] < 0f ? -maxSpeed : 0f;
hingedConstraint.maxMotorImpulse = pwm[i] == 0f ? friction : Mathf.Abs(pwm[i] * impulse);
}
}
else if (rigidNode.GetSkeletalJoint().cDriver.GetDriveType().IsElevator())
{
if (rigidNode.GetSkeletalJoint().cDriver.portA == i + 1 && rigidNode.HasDriverMeta <ElevatorDriverMeta>())
{
BSliderConstraint bSliderConstraint = rigidNode.MainObject.GetComponent <BSliderConstraint>();
SliderConstraint sc = (SliderConstraint)bSliderConstraint.GetConstraint();
sc.PoweredLinearMotor = true;
sc.MaxLinearMotorForce = MAX_SLIDER_FORCE;
sc.TargetLinearMotorVelocity = pwm[i] * MAX_SLIDER_SPEED;
}
}
}
}
}
}
public static void DebugDrawConstraint(TypedConstraint constraint, IDebugDraw debugDraw)
{
bool drawFrames = (debugDraw.GetDebugMode() & DebugDrawModes.DBG_DrawConstraints) != 0;
bool drawLimits = (debugDraw.GetDebugMode() & DebugDrawModes.DBG_DrawConstraintLimits) != 0;
float dbgDrawSize = constraint.GetDbgDrawSize();
if (dbgDrawSize <= 0f)
{
return;
}
switch (constraint.GetConstraintType())
{
case TypedConstraintType.POINT2POINT_CONSTRAINT_TYPE:
{
Point2PointConstraint p2pC = constraint as Point2PointConstraint;
IndexedMatrix tr = IndexedMatrix.Identity;
IndexedVector3 pivot = p2pC.GetPivotInA();
pivot = p2pC.GetRigidBodyA().GetCenterOfMassTransform() * pivot;
tr._origin = pivot;
debugDraw.DrawTransform(ref tr, dbgDrawSize);
// that ideally should draw the same frame
pivot = p2pC.GetPivotInB();
pivot = p2pC.GetRigidBodyB().GetCenterOfMassTransform() * pivot;
tr._origin = pivot;
if (drawFrames)
{
debugDraw.DrawTransform(ref tr, dbgDrawSize);
}
}
break;
case TypedConstraintType.HINGE_CONSTRAINT_TYPE:
{
HingeConstraint pHinge = constraint as HingeConstraint;
IndexedMatrix tr = pHinge.GetRigidBodyA().GetCenterOfMassTransform() * pHinge.GetAFrame();
if (drawFrames)
{
debugDraw.DrawTransform(ref tr, dbgDrawSize);
}
tr = pHinge.GetRigidBodyB().GetCenterOfMassTransform() * pHinge.GetBFrame();
if (drawFrames)
{
debugDraw.DrawTransform(ref tr, dbgDrawSize);
}
float minAng = pHinge.GetLowerLimit();
float maxAng = pHinge.GetUpperLimit();
if (minAng == maxAng)
{
break;
}
bool drawSect = true;
if (minAng > maxAng)
{
minAng = 0f;
maxAng = MathUtil.SIMD_2_PI;
drawSect = false;
}
if (drawLimits)
{
IndexedVector3 center = tr._origin;
IndexedVector3 normal = tr._basis.GetColumn(2);
IndexedVector3 axis = tr._basis.GetColumn(0);
IndexedVector3 zero = IndexedVector3.Zero;
debugDraw.DrawArc(ref center, ref normal, ref axis, dbgDrawSize, dbgDrawSize, minAng, maxAng, ref zero, drawSect);
}
}
break;
case TypedConstraintType.CONETWIST_CONSTRAINT_TYPE:
{
ConeTwistConstraint pCT = constraint as ConeTwistConstraint;
IndexedMatrix tr = pCT.GetRigidBodyA().GetCenterOfMassTransform() * pCT.GetAFrame();
if (drawFrames)
{
debugDraw.DrawTransform(ref tr, dbgDrawSize);
}
tr = pCT.GetRigidBodyB().GetCenterOfMassTransform() * pCT.GetBFrame();
if (drawFrames)
{
debugDraw.DrawTransform(ref tr, dbgDrawSize);
}
IndexedVector3 zero = IndexedVector3.Zero;
if (drawLimits)
{
//const float length = float(5);
float length = dbgDrawSize;
const int nSegments = 8 * 4;
float fAngleInRadians = MathUtil.SIMD_2_PI * (float)(nSegments - 1) / (float)nSegments;
IndexedVector3 pPrev = pCT.GetPointForAngle(fAngleInRadians, length);
pPrev = tr * pPrev;
for (int i = 0; i < nSegments; i++)
{
fAngleInRadians = MathUtil.SIMD_2_PI * (float)i / (float)nSegments;
IndexedVector3 pCur = pCT.GetPointForAngle(fAngleInRadians, length);
pCur = tr * pCur;
debugDraw.DrawLine(ref pPrev, ref pCur, ref zero);
if (i % (nSegments / 8) == 0)
{
IndexedVector3 origin = tr._origin;
debugDraw.DrawLine(ref origin, ref pCur, ref zero);
}
pPrev = pCur;
}
float tws = pCT.GetTwistSpan();
float twa = pCT.GetTwistAngle();
bool useFrameB = (pCT.GetRigidBodyB().GetInvMass() > 0f);
if (useFrameB)
{
tr = pCT.GetRigidBodyB().GetCenterOfMassTransform() * pCT.GetBFrame();
}
else
{
tr = pCT.GetRigidBodyA().GetCenterOfMassTransform() * pCT.GetAFrame();
}
IndexedVector3 pivot = tr._origin;
IndexedVector3 normal = tr._basis.GetColumn(0);
IndexedVector3 axis = tr._basis.GetColumn(1);
debugDraw.DrawArc(ref pivot, ref normal, ref axis, dbgDrawSize, dbgDrawSize, -twa - tws, -twa + tws, ref zero, true);
}
}
break;
case TypedConstraintType.D6_CONSTRAINT_TYPE:
case TypedConstraintType.D6_SPRING_CONSTRAINT_TYPE:
{
Generic6DofConstraint p6DOF = constraint as Generic6DofConstraint;
IndexedMatrix tr = p6DOF.GetCalculatedTransformA();
if (drawFrames)
{
debugDraw.DrawTransform(ref tr, dbgDrawSize);
}
tr = p6DOF.GetCalculatedTransformB();
if (drawFrames)
{
debugDraw.DrawTransform(ref tr, dbgDrawSize);
}
IndexedVector3 zero = IndexedVector3.Zero;
if (drawLimits)
{
tr = p6DOF.GetCalculatedTransformA();
IndexedVector3 center = p6DOF.GetCalculatedTransformB()._origin;
// up is axis 1 not 2 ?
IndexedVector3 up = tr._basis.GetColumn(1);
IndexedVector3 axis = tr._basis.GetColumn(0);
float minTh = p6DOF.GetRotationalLimitMotor(1).m_loLimit;
float maxTh = p6DOF.GetRotationalLimitMotor(1).m_hiLimit;
float minPs = p6DOF.GetRotationalLimitMotor(2).m_loLimit;
float maxPs = p6DOF.GetRotationalLimitMotor(2).m_hiLimit;
debugDraw.DrawSpherePatch(ref center, ref up, ref axis, dbgDrawSize * .9f, minTh, maxTh, minPs, maxPs, ref zero);
axis = tr._basis.GetColumn(1);
float ay = p6DOF.GetAngle(1);
float az = p6DOF.GetAngle(2);
float cy = (float)Math.Cos(ay);
float sy = (float)Math.Sin(ay);
float cz = (float)Math.Cos(az);
float sz = (float)Math.Sin(az);
IndexedVector3 ref1 = new IndexedVector3(
cy * cz * axis.X + cy * sz * axis.Y - sy * axis.Z,
-sz * axis.X + cz * axis.Y,
cz * sy * axis.X + sz * sy * axis.Y + cy * axis.Z);
tr = p6DOF.GetCalculatedTransformB();
IndexedVector3 normal = -tr._basis.GetColumn(0);
float minFi = p6DOF.GetRotationalLimitMotor(0).m_loLimit;
float maxFi = p6DOF.GetRotationalLimitMotor(0).m_hiLimit;
if (minFi > maxFi)
{
debugDraw.DrawArc(ref center, ref normal, ref ref1, dbgDrawSize, dbgDrawSize, -MathUtil.SIMD_PI, MathUtil.SIMD_PI, ref zero, false);
}
else if (minFi < maxFi)
{
debugDraw.DrawArc(ref center, ref normal, ref ref1, dbgDrawSize, dbgDrawSize, minFi, maxFi, ref zero, false);
}
tr = p6DOF.GetCalculatedTransformA();
IndexedVector3 bbMin = p6DOF.GetTranslationalLimitMotor().m_lowerLimit;
IndexedVector3 bbMax = p6DOF.GetTranslationalLimitMotor().m_upperLimit;
debugDraw.DrawBox(ref bbMin, ref bbMax, ref tr, ref zero);
}
}
break;
case TypedConstraintType.SLIDER_CONSTRAINT_TYPE:
{
SliderConstraint pSlider = constraint as SliderConstraint;
IndexedMatrix tr = pSlider.GetCalculatedTransformA();
if (drawFrames)
{
debugDraw.DrawTransform(ref tr, dbgDrawSize);
}
tr = pSlider.GetCalculatedTransformB();
if (drawFrames)
{
debugDraw.DrawTransform(ref tr, dbgDrawSize);
}
IndexedVector3 zero = IndexedVector3.Zero;
if (drawLimits)
{
IndexedMatrix tr2 = pSlider.GetCalculatedTransformA();
IndexedVector3 li_min = tr2 * new IndexedVector3(pSlider.GetLowerLinLimit(), 0f, 0f);
IndexedVector3 li_max = tr2 * new IndexedVector3(pSlider.GetUpperLinLimit(), 0f, 0f);
debugDraw.DrawLine(ref li_min, ref li_max, ref zero);
IndexedVector3 normal = tr._basis.GetColumn(0);
IndexedVector3 axis = tr._basis.GetColumn(1);
float a_min = pSlider.GetLowerAngLimit();
float a_max = pSlider.GetUpperAngLimit();
IndexedVector3 center = pSlider.GetCalculatedTransformB()._origin;
debugDraw.DrawArc(ref center, ref normal, ref axis, dbgDrawSize, dbgDrawSize, a_min, a_max, ref zero, true);
}
}
break;
default:
break;
}
return;
}
//----------------------------------------------------------------------------------------------------------------
public override void InitializeDemo()
{
CollisionShape groundShape = new BoxShape(new IndexedVector3(50, 3, 50));
//CollisionShape groundShape = new StaticPlaneShape(IndexedVector3.Up, 0f);
m_collisionShapes.Add(groundShape);
m_collisionConfiguration = new DefaultCollisionConfiguration();
m_dispatcher = new CollisionDispatcher(m_collisionConfiguration);
IndexedVector3 worldMin = new IndexedVector3(-1000, -1000, -1000);
IndexedVector3 worldMax = new IndexedVector3(1000, 1000, 1000);
//m_broadphase = new AxisSweep3Internal(ref worldMin, ref worldMax, 0xfffe, 0xffff, 16384, null, false);
m_broadphase = new SimpleBroadphase(100, null);
m_constraintSolver = new SequentialImpulseConstraintSolver();
m_dynamicsWorld = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_constraintSolver, m_collisionConfiguration);
//m_dynamicsWorld.setGravity(new IndexedVector3(0,0,0));
IndexedMatrix tr = IndexedMatrix.CreateTranslation(0, -10, 0);
//either use heightfield or triangle mesh
//create ground object
LocalCreateRigidBody(0f, ref tr, groundShape);
CollisionShape chassisShape = new BoxShape(new IndexedVector3(1.0f, 0.5f, 2.0f));
m_collisionShapes.Add(chassisShape);
CompoundShape compound = new CompoundShape();
m_collisionShapes.Add(compound);
//localTrans effectively shifts the center of mass with respect to the chassis
IndexedMatrix localTrans = IndexedMatrix.CreateTranslation(0, 1, 0);
compound.AddChildShape(ref localTrans, chassisShape);
{
CollisionShape suppShape = new BoxShape(new IndexedVector3(0.5f, 0.1f, 0.5f));
//localTrans effectively shifts the center of mass with respect to the chassis
IndexedMatrix suppLocalTrans = IndexedMatrix.CreateTranslation(0f, 1.0f, 2.5f);
compound.AddChildShape(ref suppLocalTrans, suppShape);
}
tr._origin = IndexedVector3.Zero;
m_carChassis = LocalCreateRigidBody(800f, ref tr, compound);//chassisShape);
//m_carChassis = LocalCreateRigidBody(800f, ref tr, chassisShape);//chassisShape);
//CollisionShape liftShape = new BoxShape(new IndexedVector3(0.5f, 2.0f, 0.05f));
//m_collisionShapes.Add(liftShape);
//m_liftStartPos = new IndexedVector3(0.0f, 2.5f, 3.05f);
//IndexedMatrix liftTrans = IndexedMatrix.CreateTranslation(m_liftStartPos);
//m_liftBody = LocalCreateRigidBody(10f, ref liftTrans, liftShape);
//IndexedMatrix localA = MathUtil.SetEulerZYX(0f, MathUtil.SIMD_HALF_PI, 0f);
//localA._origin = new IndexedVector3(0f, 1.0f, 3.05f);
//IndexedMatrix localB = MathUtil.SetEulerZYX(0f, MathUtil.SIMD_HALF_PI, 0f);
//localB._origin = new IndexedVector3(0f, -1.5f, -0.05f);
//m_liftHinge = new HingeConstraint(m_carChassis, m_liftBody, ref localA, ref localB);
//// m_liftHinge.setLimit(-LIFT_EPS, LIFT_EPS);
//m_liftHinge.SetLimit(0.0f, 0.0f);
//m_dynamicsWorld.AddConstraint(m_liftHinge, true);
//CompoundShape forkCompound = new CompoundShape();
//m_collisionShapes.Add(forkCompound);
//IndexedMatrix forkLocalTrans = IndexedMatrix.Identity;
//CollisionShape forkShapeA = new BoxShape(new IndexedVector3(1.0f, 0.1f, 0.1f));
//m_collisionShapes.Add(forkShapeA);
//forkCompound.AddChildShape(ref forkLocalTrans, forkShapeA);
//CollisionShape forkShapeB = new BoxShape(new IndexedVector3(0.1f, 0.02f, 0.6f));
//m_collisionShapes.Add(forkShapeB);
//forkLocalTrans = IndexedMatrix.CreateTranslation(-0.9f, -0.08f, 0.7f);
//forkCompound.AddChildShape(ref forkLocalTrans, forkShapeB);
//CollisionShape forkShapeC = new BoxShape(new IndexedVector3(0.1f, 0.02f, 0.6f));
//m_collisionShapes.Add(forkShapeC);
//forkLocalTrans = IndexedMatrix.CreateTranslation(0.9f, -0.08f, 0.7f);
//forkCompound.AddChildShape(ref forkLocalTrans, forkShapeC);
//m_forkStartPos = new IndexedVector3(0.0f, 0.6f, 3.2f);
//IndexedMatrix forkTrans = IndexedMatrix.CreateTranslation(m_forkStartPos);
//m_forkBody = LocalCreateRigidBody(5f, ref forkTrans, forkCompound);
//localA = MathUtil.SetEulerZYX(0f, 0f, MathUtil.SIMD_HALF_PI);
//localA._origin = new IndexedVector3(0.0f, -1.9f, 0.05f);
//IndexedVector3 col0 = MathUtil.matrixColumn(ref localA, 0);
//IndexedVector3 col1 = MathUtil.matrixColumn(ref localA, 1);
//IndexedVector3 col2 = MathUtil.matrixColumn(ref localA, 2);
////localB = MathUtil.setEulerZYX(0f, 0f, MathUtil.SIMD_HALF_PI);
//localB = MathUtil.SetEulerZYX(0f, 0f, MathUtil.SIMD_HALF_PI);
//localB._origin = new IndexedVector3(0.0f, 0.0f, -0.1f);
//m_forkSlider = new SliderConstraint(m_liftBody, m_forkBody, ref localA, ref localB, true);
//m_forkSlider.SetLowerLinLimit(0.1f);
//m_forkSlider.SetUpperLinLimit(0.1f);
//// m_forkSlider.setLowerAngLimit(-LIFT_EPS);
//// m_forkSlider.setUpperAngLimit(LIFT_EPS);
//m_forkSlider.SetLowerAngLimit(0.0f);
//m_forkSlider.SetUpperAngLimit(0.0f);
//IndexedMatrix localAVec = IndexedMatrix.Identity;
//IndexedMatrix localBVec = IndexedMatrix.Identity;
//m_forkSlider2 = new HingeConstraint(m_liftBody, m_forkBody, ref localAVec, ref localBVec);
//m_dynamicsWorld.AddConstraint(m_forkSlider, true);
//m_dynamicsWorld.addConstraint(m_forkSlider2, true);
CompoundShape loadCompound = new CompoundShape(true);
m_collisionShapes.Add(loadCompound);
CollisionShape loadShapeA = new BoxShape(new IndexedVector3(2.0f, 0.5f, 0.5f));
m_collisionShapes.Add(loadShapeA);
IndexedMatrix loadTrans = IndexedMatrix.Identity;
loadCompound.AddChildShape(ref loadTrans, loadShapeA);
CollisionShape loadShapeB = new BoxShape(new IndexedVector3(0.1f, 1.0f, 1.0f));
m_collisionShapes.Add(loadShapeB);
loadTrans = IndexedMatrix.CreateTranslation(2.1f, 0.0f, 0.0f);
loadCompound.AddChildShape(ref loadTrans, loadShapeB);
CollisionShape loadShapeC = new BoxShape(new IndexedVector3(0.1f, 1.0f, 1.0f));
m_collisionShapes.Add(loadShapeC);
loadTrans = IndexedMatrix.CreateTranslation(-2.1f, 0.0f, 0.0f);
loadCompound.AddChildShape(ref loadTrans, loadShapeC);
m_loadStartPos = new IndexedVector3(0.0f, -3.5f, 7.0f);
loadTrans = IndexedMatrix.CreateTranslation(m_loadStartPos);
m_loadBody = LocalCreateRigidBody(4f, ref loadTrans, loadCompound);
#if false
{
CollisionShape liftShape = new BoxShape(new IndexedVector3(0.5f, 2.0f, 0.05f));
m_collisionShapes.Add(liftShape);
IndexedMatrix liftTrans = IndexedMatrix.CreateTranslation(m_liftStartPos);
m_liftBody = localCreateRigidBody(10f, ref liftTrans, liftShape);
IndexedMatrix localA = MathUtil.setEulerZYX(0f, MathUtil.SIMD_HALF_PI, 0f);
localA._origin = new IndexedVector3(0f, 1.0f, 3.05f);
IndexedMatrix localB = MathUtil.setEulerZYX(0f, MathUtil.SIMD_HALF_PI, 0f);
localB._origin = new IndexedVector3(0f, -1.5f, -0.05f);
m_liftHinge = new HingeConstraint(m_carChassis, m_liftBody, ref localA, ref localB);
// m_liftHinge.setLimit(-LIFT_EPS, LIFT_EPS);
m_liftHinge.setLimit(0.0f, 0.0f);
m_dynamicsWorld.addConstraint(m_liftHinge, true);
CollisionShape forkShapeA = new BoxShape(new IndexedVector3(1.0f, 0.1f, 0.1f));
m_collisionShapes.Add(forkShapeA);
CompoundShape forkCompound = new CompoundShape();
m_collisionShapes.Add(forkCompound);
IndexedMatrix forkLocalTrans = IndexedMatrix.Identity;
forkCompound.addChildShape(ref forkLocalTrans, forkShapeA);
CollisionShape forkShapeB = new BoxShape(new IndexedVector3(0.1f, 0.02f, 0.6f));
m_collisionShapes.Add(forkShapeB);
forkLocalTrans = IndexedMatrix.CreateTranslation(-0.9f, -0.08f, 0.7f);
forkCompound.addChildShape(ref forkLocalTrans, forkShapeB);
CollisionShape forkShapeC = new BoxShape(new IndexedVector3(0.1f, 0.02f, 0.6f));
m_collisionShapes.Add(forkShapeC);
forkLocalTrans = IndexedMatrix.CreateTranslation(0.9f, -0.08f, 0.7f);
forkCompound.addChildShape(ref forkLocalTrans, forkShapeC);
m_forkStartPos = new IndexedVector3(0.0f, 0.6f, 3.2f);
IndexedMatrix forkTrans = IndexedMatrix.CreateTranslation(m_forkStartPos);
m_forkBody = localCreateRigidBody(5f, ref forkTrans, forkCompound);
localA = MathUtil.setEulerZYX(0f, 0f, MathUtil.SIMD_HALF_PI);
localA._origin = new IndexedVector3(0.0f, -1.9f, 0.05f);
localB = MathUtil.setEulerZYX(0f, 0f, MathUtil.SIMD_HALF_PI);
localB._origin = new IndexedVector3(0.0f, 0.0f, -0.1f);
m_forkSlider = new SliderConstraint(m_liftBody, m_forkBody, ref localA, ref localB, true);
m_forkSlider.setLowerLinLimit(0.1f);
m_forkSlider.setUpperLinLimit(0.1f);
// m_forkSlider.setLowerAngLimit(-LIFT_EPS);
// m_forkSlider.setUpperAngLimit(LIFT_EPS);
m_forkSlider.setLowerAngLimit(0.0f);
m_forkSlider.setUpperAngLimit(0.0f);
m_dynamicsWorld.addConstraint(m_forkSlider, true);
CompoundShape loadCompound = new CompoundShape();
m_collisionShapes.Add(loadCompound);
CollisionShape loadShapeA = new BoxShape(new IndexedVector3(2.0f, 0.5f, 0.5f));
m_collisionShapes.Add(loadShapeA);
IndexedMatrix loadTrans = IndexedMatrix.Identity;
loadCompound.addChildShape(ref loadTrans, loadShapeA);
CollisionShape loadShapeB = new BoxShape(new IndexedVector3(0.1f, 1.0f, 1.0f));
m_collisionShapes.Add(loadShapeB);
loadTrans = IndexedMatrix.CreateTranslation(2.1f, 0.0f, 0.0f);
loadCompound.addChildShape(ref loadTrans, loadShapeB);
CollisionShape loadShapeC = new BoxShape(new IndexedVector3(0.1f, 1.0f, 1.0f));
m_collisionShapes.Add(loadShapeC);
loadTrans = IndexedMatrix.CreateTranslation(-2.1f, 0.0f, 0.0f);
loadCompound.addChildShape(ref loadTrans, loadShapeC);
m_loadStartPos = new IndexedVector3(0.0f, -3.5f, 7.0f);
loadTrans = IndexedMatrix.CreateTranslation(m_loadStartPos);
m_loadBody = localCreateRigidBody(4f, ref loadTrans, loadCompound);
}
#endif
//m_carChassis.setDamping(0.2f, 0.2f);
ClientResetScene();
/// create vehicle
SetCameraDistance(26.0f);
SetTexturing(true);
SetShadows(true);
}
//----------------------------------------------------------------------------------------------------------------
public override void InitializeDemo()
{
CollisionShape groundShape = new BoxShape(new IndexedVector3(50, 0.1f, 50));
IndexedVector3 wheelDimensions = new IndexedVector3(wheelWidth, wheelRadius, wheelRadius);
m_wheelShape = new CylinderShapeX(ref wheelDimensions);
m_collisionShapes.Add(groundShape);
m_collisionConfiguration = new DefaultCollisionConfiguration();
m_dispatcher = new CollisionDispatcher(m_collisionConfiguration);
IndexedVector3 worldMin = new IndexedVector3(-1000, -1000, -1000);
IndexedVector3 worldMax = new IndexedVector3(1000, 1000, 1000);
m_broadphase = new AxisSweep3Internal(ref worldMin, ref worldMax, 0xfffe, 0xffff, 16384, null, false);
m_constraintSolver = new SequentialImpulseConstraintSolver();
m_dynamicsWorld = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_constraintSolver, m_collisionConfiguration);
IndexedMatrix tr = IndexedMatrix.CreateTranslation(0, -10, 0);
LocalCreateRigidBody(0f, ref tr, groundShape);
#if true
CollisionShape chassisShape = new BoxShape(new IndexedVector3(1.0f, 0.5f, 2.0f));
//CollisionShape chassisShape = new BoxShape(new IndexedVector3(1.0f, 0.5f, 1.0f));
m_collisionShapes.Add(chassisShape);
CompoundShape compound = new CompoundShape();
m_collisionShapes.Add(compound);
//localTrans effectively shifts the center of mass with respect to the chassis
IndexedMatrix localTrans = IndexedMatrix.CreateTranslation(0, 1, 0);
compound.AddChildShape(ref localTrans, chassisShape);
{
CollisionShape suppShape = new BoxShape(new IndexedVector3(0.5f, 0.1f, 0.5f));
//localTrans effectively shifts the center of mass with respect to the chassis
IndexedMatrix suppLocalTrans = IndexedMatrix.CreateTranslation(0f, 1.0f, 2.5f);
compound.AddChildShape(ref suppLocalTrans, suppShape);
}
tr._origin = IndexedVector3.Zero;
m_carChassis = LocalCreateRigidBody(800f, ref tr, compound);//chassisShape);
#endif
{
#if true
CollisionShape liftShape = new BoxShape(new IndexedVector3(0.5f, 2.0f, 0.05f));
m_collisionShapes.Add(liftShape);
m_liftStartPos = new IndexedVector3(0.0f, 2.5f, 3.05f);
IndexedMatrix liftTrans = IndexedMatrix.CreateTranslation(m_liftStartPos);
m_liftBody = LocalCreateRigidBody(10f, ref liftTrans, liftShape);
IndexedMatrix localA = MathUtil.SetEulerZYX(0f, MathUtil.SIMD_HALF_PI, 0f);
localA._origin = new IndexedVector3(0f, 1.0f, 3.05f);
IndexedMatrix localB = MathUtil.SetEulerZYX(0f, MathUtil.SIMD_HALF_PI, 0f);
localB._origin = new IndexedVector3(0f, -1.5f, -0.05f);
m_liftHinge = new HingeConstraint(m_carChassis, m_liftBody, ref localA, ref localB);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "hinge aFrame", m_liftHinge.GetAFrame());
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "hinge bFrame", m_liftHinge.GetBFrame());
m_liftHinge.SetLimit(0.0f, 0.0f);
m_dynamicsWorld.AddConstraint(m_liftHinge, true);
CollisionShape forkShapeA = new BoxShape(new IndexedVector3(1.0f, 0.1f, 0.1f));
m_collisionShapes.Add(forkShapeA);
CompoundShape forkCompound = new CompoundShape();
m_collisionShapes.Add(forkCompound);
IndexedMatrix forkLocalTrans = IndexedMatrix.Identity;
forkCompound.AddChildShape(ref forkLocalTrans, forkShapeA);
CollisionShape forkShapeB = new BoxShape(new IndexedVector3(0.1f, 0.02f, 0.6f));
m_collisionShapes.Add(forkShapeB);
forkLocalTrans = IndexedMatrix.CreateTranslation(-0.9f, -0.08f, 0.7f);
forkCompound.AddChildShape(ref forkLocalTrans, forkShapeB);
CollisionShape forkShapeC = new BoxShape(new IndexedVector3(0.1f, 0.02f, 0.6f));
m_collisionShapes.Add(forkShapeC);
forkLocalTrans = IndexedMatrix.CreateTranslation(0.9f, -0.08f, 0.7f);
forkCompound.AddChildShape(ref forkLocalTrans, forkShapeC);
m_forkStartPos = new IndexedVector3(0.0f, 0.6f, 3.2f);
IndexedMatrix forkTrans = IndexedMatrix.CreateTranslation(m_forkStartPos);
m_forkBody = LocalCreateRigidBody(5f, ref forkTrans, forkCompound);
localA = IndexedMatrix.Identity;
localB = IndexedMatrix.Identity;
localA._basis.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
localA._origin = new IndexedVector3(0.0f, -1.9f, 0.05f);
localB._basis.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
localB._origin = new IndexedVector3(0.0f, 0.0f, -0.1f);
m_forkSlider = new SliderConstraint(m_liftBody, m_forkBody, ref localA, ref localB, true);
m_forkSlider.SetLowerLinLimit(0.1f);
m_forkSlider.SetUpperLinLimit(0.1f);
// m_forkSlider.setLowerAngLimit(-LIFT_EPS);
// m_forkSlider.setUpperAngLimit(LIFT_EPS);
m_forkSlider.SetLowerAngLimit(0.0f);
m_forkSlider.SetUpperAngLimit(0.0f);
m_dynamicsWorld.AddConstraint(m_forkSlider, true);
#endif
#if true
CompoundShape loadCompound = new CompoundShape(false);
m_collisionShapes.Add(loadCompound);
CollisionShape loadShapeA = new BoxShape(new IndexedVector3(2.0f, 0.5f, 0.5f));
m_collisionShapes.Add(loadShapeA);
IndexedMatrix loadTrans = IndexedMatrix.Identity;
loadCompound.AddChildShape(ref loadTrans, loadShapeA);
CollisionShape loadShapeB = new BoxShape(new IndexedVector3(0.1f, 1.0f, 1.0f));
m_collisionShapes.Add(loadShapeB);
loadTrans = IndexedMatrix.CreateTranslation(2.1f, 0.0f, 0.0f);
loadCompound.AddChildShape(ref loadTrans, loadShapeB);
CollisionShape loadShapeC = new BoxShape(new IndexedVector3(0.1f, 1.0f, 1.0f));
m_collisionShapes.Add(loadShapeC);
loadTrans = IndexedMatrix.CreateTranslation(-2.1f, 0.0f, 0.0f);
loadCompound.AddChildShape(ref loadTrans, loadShapeC);
m_loadStartPos = new IndexedVector3(0.0f, -3.5f, 7.0f);
loadTrans = IndexedMatrix.CreateTranslation(m_loadStartPos);
m_loadBody = LocalCreateRigidBody(4f, ref loadTrans, loadCompound);
#endif
}
//m_carChassis.setDamping(0.2f, 0.2f);
ClientResetScene();
/// create vehicle
{
m_vehicleRayCaster = new DefaultVehicleRaycaster(m_dynamicsWorld);
m_vehicle = new RaycastVehicle(m_tuning, m_carChassis, m_vehicleRayCaster);
///never deactivate the vehicle
m_carChassis.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
m_dynamicsWorld.AddVehicle(m_vehicle);
float connectionHeight = 1.2f;
bool isFrontWheel = true;
//choose coordinate system
m_vehicle.SetCoordinateSystem(rightIndex, upIndex, forwardIndex);
IndexedVector3 connectionPointCS0 = IndexedVector3.Zero;
//connectionPointCS0 = new IndexedVector3(CUBE_HALF_EXTENTS, connectionHeight, CUBE_HALF_EXTENTS);
//m_vehicle.addWheel(ref connectionPointCS0, ref wheelDirectionCS0, ref wheelAxleCS, suspensionRestLength, wheelRadius, m_tuning, isFrontWheel);
connectionPointCS0 = new IndexedVector3(CUBE_HALF_EXTENTS - (0.3f * wheelWidth), connectionHeight, 2.0f * CUBE_HALF_EXTENTS - wheelRadius);
m_vehicle.AddWheel(ref connectionPointCS0, ref wheelDirectionCS0, ref wheelAxleCS, suspensionRestLength, wheelRadius, m_tuning, isFrontWheel);
connectionPointCS0 = new IndexedVector3(-CUBE_HALF_EXTENTS + (0.3f * wheelWidth), connectionHeight, 2.0f * CUBE_HALF_EXTENTS - wheelRadius);
m_vehicle.AddWheel(ref connectionPointCS0, ref wheelDirectionCS0, ref wheelAxleCS, suspensionRestLength, wheelRadius, m_tuning, isFrontWheel);
//isFrontWheel = false;
connectionPointCS0 = new IndexedVector3(-CUBE_HALF_EXTENTS + (0.3f * wheelWidth), connectionHeight, -2.0f * CUBE_HALF_EXTENTS + wheelRadius);
m_vehicle.AddWheel(ref connectionPointCS0, ref wheelDirectionCS0, ref wheelAxleCS, suspensionRestLength, wheelRadius, m_tuning, isFrontWheel);
connectionPointCS0 = new IndexedVector3(CUBE_HALF_EXTENTS - (0.3f * wheelWidth), connectionHeight, -2.0f * CUBE_HALF_EXTENTS + wheelRadius);
m_vehicle.AddWheel(ref connectionPointCS0, ref wheelDirectionCS0, ref wheelAxleCS, suspensionRestLength, wheelRadius, m_tuning, isFrontWheel);
for (int i = 0; i < m_vehicle.GetNumWheels(); i++)
{
WheelInfo wheel = m_vehicle.GetWheelInfo(i);
wheel.m_suspensionStiffness = suspensionStiffness;
wheel.m_wheelsDampingRelaxation = suspensionDamping;
wheel.m_wheelsDampingCompression = suspensionCompression;
wheel.m_frictionSlip = wheelFriction;
wheel.m_rollInfluence = rollInfluence;
}
}
SetCameraDistance(36.0f);
SetTexturing(true);
SetShadows(true);
}
/// <summary>
/// Updates the motors on the manipulator in mix and match mode. Called every frame.
/// </summary>
/// <param name="skeleton"></param>
/// <param name="dioModules"></param>
/// <param name="controlIndex"></param>
public static void UpdateManipulatorMotors(RigidNode_Base skeleton, UnityPacket.OutputStatePacket.DIOModule[] dioModules, int controlIndex)
{
float[] pwm;
float[] can;
if (dioModules[0] != null)
{
pwm = dioModules[0].pwmValues;
can = dioModules[0].canValues;
}
else
{
pwm = new float[10];
can = new float[10];
}
pwm[4] +=
(InputControl.GetAxis(Controls.axes[controlIndex].pwm4Axes) * SpeedArrowPwm);
pwm[5] +=
(InputControl.GetAxis(Controls.axes[controlIndex].pwm5Axes) * SpeedArrowPwm);
pwm[6] +=
(InputControl.GetAxis(Controls.axes[controlIndex].pwm6Axes) * SpeedArrowPwm);
listOfSubNodes.Clear();
skeleton.ListAllNodes(listOfSubNodes);
for (int i = 0; i < pwm.Length; i++)
{
foreach (RigidNode_Base node in listOfSubNodes)
{
RigidNode rigidNode = (RigidNode)node;
BRaycastWheel raycastWheel = rigidNode.MainObject.GetComponent <BRaycastWheel>();
SkeletalJoint_Base joint = rigidNode.GetSkeletalJoint();
if (raycastWheel != null)
{
if (joint.cDriver.port1 == i + 1)
{
float output = pwm[i];
MotorType motorType = joint.cDriver.GetMotorType();
float torque = motorType == MotorType.GENERIC ? 2.42f : 60 * motorDefinition[motorType].baseTorque - motorDefinition[motorType].slope * raycastWheel.GetWheelSpeed() / 9.549297f;
if (joint.cDriver.InputGear != 0 && joint.cDriver.OutputGear != 0)
{
torque /= Convert.ToSingle(joint.cDriver.InputGear / joint.cDriver.OutputGear);
}
raycastWheel.ApplyForce(output, torque, motorType == MotorType.GENERIC);
}
}
if (joint != null && joint.cDriver != null)
{
if (joint.cDriver.GetDriveType().IsMotor() && rigidNode.MainObject.GetComponent <BHingedConstraint>() != null)
{
if (joint.cDriver.port1 == i + 1)
{
float maxSpeed = 0f;
float impulse = 0f;
float friction = 0f;
friction = HingeCostFriction;
MotorType motorType = joint.cDriver.GetMotorType();
Motor motor = motorType == MotorType.GENERIC ? new Motor(10f, 4f) : motorDefinition[motorType];
maxSpeed = motor.maxSpeed;
impulse = motor.baseTorque - motor.slope * ((RigidBody)(rigidNode.MainObject.GetComponent <BRigidBody>().GetCollisionObject())).AngularVelocity.Length / 9.549297f;
if (joint.cDriver.InputGear != 0 && joint.cDriver.OutputGear != 0)
{
float gearRatio = Convert.ToSingle(joint.cDriver.InputGear / joint.cDriver.OutputGear);
impulse /= gearRatio;
maxSpeed *= gearRatio;
}
BHingedConstraint hingedConstraint = rigidNode.MainObject.GetComponent <BHingedConstraint>();
hingedConstraint.enableMotor = true;
hingedConstraint.targetMotorAngularVelocity = pwm[i] > 0f ? maxSpeed : pwm[i] < 0f ? -maxSpeed : 0f;
hingedConstraint.maxMotorImpulse = joint.cDriver.hasBrake ? motor.baseTorque : pwm[i] == 0f ? friction : Mathf.Abs(pwm[i] * impulse);
}
}
else if (joint.cDriver.GetDriveType().IsElevator())
{
if (joint.cDriver.port1 == i + 1 && rigidNode.HasDriverMeta <ElevatorDriverMeta>())
{
BSliderConstraint bSliderConstraint = rigidNode.MainObject.GetComponent <BSliderConstraint>();
SliderConstraint sc = (SliderConstraint)bSliderConstraint.GetConstraint();
sc.PoweredLinearMotor = true;
sc.MaxLinearMotorForce = MaxSliderForce;
sc.TargetLinearMotorVelocity = pwm[i] * MaxSliderSpeed;
}
}
else if (joint.cDriver.GetDriveType().IsPneumatic() && rigidNode.HasDriverMeta <PneumaticDriverMeta>())
{
BSliderConstraint bSliderConstraint = rigidNode.MainObject.GetComponent <BSliderConstraint>();
SliderConstraint sc = (SliderConstraint)bSliderConstraint.GetConstraint();
float output = motors[joint.cDriver.port1 - 1];
float psi = node.GetDriverMeta <PneumaticDriverMeta>().pressurePSI * 6894.76f;
float width = node.GetDriverMeta <PneumaticDriverMeta>().widthMM * 0.001f;
float stroke = (sc.UpperLinearLimit - sc.LowerLinearLimit) / 0.01f;
float force = psi * ((float)Math.PI) * width * width / 4f;
float speed = stroke / 60f;
sc.PoweredLinearMotor = true;
sc.MaxLinearMotorForce = force;
sc.TargetLinearMotorVelocity = sc.TargetLinearMotorVelocity != 0 && output == 0 ? sc.TargetLinearMotorVelocity : output * speed;
}
}
}
}
}
/// <summary>
/// Updates all motor values from the given <see cref="RigidNode_Base"/>, pwm values, and emulation network info.
/// </summary>
/// <param name="skeleton"></param>
/// <param name="pwm"></param>
/// <param name="emuList"></param>
public static void UpdateAllMotors(RigidNode_Base skeleton, float[] pwm, List <Synthesis.Robot.RobotBase.EmuNetworkInfo> emuList)
{
listOfSubNodes.Clear();
skeleton.ListAllNodes(listOfSubNodes);
for (int i = 0; i < pwm.Length; i++)
{
motors[i] = pwm[i];
}
if (Synthesis.GUI.EmulationDriverStation.Instance != null)
{
UpdateEmulationJoysticks();
UpdateEmulationMotors(pwm);
UpdateEmulationSensors(emuList);
}
foreach (RigidNode node in listOfSubNodes.Select(n => n as RigidNode))
{
SkeletalJoint_Base joint = node.GetSkeletalJoint();
if (joint == null || joint.cDriver == null)
{
continue;
}
BRaycastWheel raycastWheel = node.MainObject.GetComponent <BRaycastWheel>();
if (raycastWheel != null)
{
float output = motors[node.GetSkeletalJoint().cDriver.port1 - 1];
MotorType motorType = joint.cDriver.GetMotorType();
float torque = motorType == MotorType.GENERIC ? 2.42f : 60 * motorDefinition[motorType].baseTorque - motorDefinition[motorType].slope * raycastWheel.GetWheelSpeed() / 9.549297f;
if (joint.cDriver.InputGear != 0 && joint.cDriver.OutputGear != 0)
{
torque /= Convert.ToSingle(joint.cDriver.InputGear / joint.cDriver.OutputGear);
}
raycastWheel.ApplyForce(output, torque, motorType == MotorType.GENERIC);
}
else if (joint.cDriver.GetDriveType().IsMotor() && node.MainObject.GetComponent <BHingedConstraint>() != null)
{
float maxSpeed = 0f;
float impulse = 0f;
float friction = 0f;
float output = !joint.cDriver.isCan ? motors[joint.cDriver.port1 - 1] : motors[joint.cDriver.port1 - 10];
friction = HingeCostFriction;
MotorType motorType = joint.cDriver.GetMotorType();
Motor motor = motorType == MotorType.GENERIC ? new Motor(10f, 4f) : motorDefinition[motorType];
maxSpeed = motor.maxSpeed;
impulse = motor.baseTorque - motor.slope * ((RigidBody)(node.MainObject.GetComponent <BRigidBody>().GetCollisionObject())).AngularVelocity.Length / 9.549297f;
if (joint.cDriver.InputGear != 0 && joint.cDriver.OutputGear != 0)
{
float gearRatio = Convert.ToSingle(joint.cDriver.InputGear / joint.cDriver.OutputGear);
impulse /= gearRatio;
maxSpeed *= gearRatio;
}
BHingedConstraint hingedConstraint = node.MainObject.GetComponent <BHingedConstraint>();
hingedConstraint.enableMotor = true;
hingedConstraint.targetMotorAngularVelocity = output > 0f ? maxSpeed : output < 0f ? -maxSpeed : 0f;
hingedConstraint.maxMotorImpulse = node.GetSkeletalJoint().cDriver.hasBrake ? motor.baseTorque : output == 0f ? friction : Mathf.Abs(output * impulse);
}
else if (joint.cDriver.GetDriveType().IsElevator() && node.HasDriverMeta <ElevatorDriverMeta>())
{
float output = motors[joint.cDriver.port1 - 1];
BSliderConstraint bSliderConstraint = node.MainObject.GetComponent <BSliderConstraint>();
SliderConstraint sc = (SliderConstraint)bSliderConstraint.GetConstraint();
sc.PoweredLinearMotor = true;
sc.MaxLinearMotorForce = MaxSliderForce;
sc.TargetLinearMotorVelocity = output * MaxSliderSpeed;
}
else if (joint.cDriver.GetDriveType().IsPneumatic() && node.HasDriverMeta <PneumaticDriverMeta>())
{
BSliderConstraint bSliderConstraint = node.MainObject.GetComponent <BSliderConstraint>();
SliderConstraint sc = (SliderConstraint)bSliderConstraint.GetConstraint();
float output = motors[joint.cDriver.port1 - 1];
float psi = node.GetDriverMeta <PneumaticDriverMeta>().pressurePSI * 6894.76f;
float width = node.GetDriverMeta <PneumaticDriverMeta>().widthMM * 0.001f;
float stroke = (sc.UpperLinearLimit - sc.LowerLinearLimit) / 0.01f;
float force = psi * ((float)Math.PI) * width * width / 4f;
float speed = stroke / 60f;
sc.PoweredLinearMotor = true;
sc.MaxLinearMotorForce = force;
sc.TargetLinearMotorVelocity = sc.TargetLinearMotorVelocity != 0 && output == 0 ? sc.TargetLinearMotorVelocity : output * speed;
}
}
}
/// <summary>
/// Updates the motors on the manipulator in mix and match mode. Called every frame.
/// </summary>
/// <param name="skeleton"></param>
/// <param name="dioModules"></param>
/// <param name="controlIndex"></param>
public static void UpdateManipulatorMotors(RigidNode_Base skeleton, UnityPacket.OutputStatePacket.DIOModule[] dioModules, int controlIndex)
{
float[] pwm;
float[] can;
if (dioModules[0] != null)
{
pwm = dioModules[0].pwmValues;
can = dioModules[0].canValues;
}
else
{
pwm = new float[10];
can = new float[10];
}
pwm[4] +=
(InputControl.GetAxis(Controls.axes[controlIndex].pwm4Axes) * SpeedArrowPwm);
pwm[5] +=
(InputControl.GetAxis(Controls.axes[controlIndex].pwm5Axes) * SpeedArrowPwm);
pwm[6] +=
(InputControl.GetAxis(Controls.axes[controlIndex].pwm6Axes) * SpeedArrowPwm);
listOfSubNodes.Clear();
skeleton.ListAllNodes(listOfSubNodes);
for (int i = 0; i < pwm.Length; i++)
{
foreach (RigidNode_Base node in listOfSubNodes)
{
RigidNode rigidNode = (RigidNode)node;
BRaycastWheel raycastWheel = rigidNode.MainObject.GetComponent <BRaycastWheel>();
if (raycastWheel != null)
{
if (rigidNode.GetSkeletalJoint().cDriver.port1 == i + 1)
{
float force = pwm[i];
if (rigidNode.GetSkeletalJoint().cDriver.InputGear != 0 && rigidNode.GetSkeletalJoint().cDriver.OutputGear != 0)
{
force *= Convert.ToSingle(rigidNode.GetSkeletalJoint().cDriver.InputGear / rigidNode.GetSkeletalJoint().cDriver.OutputGear);
}
raycastWheel.ApplyForce(force);
}
}
if (rigidNode.GetSkeletalJoint() != null && rigidNode.GetSkeletalJoint().cDriver != null)
{
if (rigidNode.GetSkeletalJoint().cDriver.GetDriveType().IsMotor() && rigidNode.MainObject.GetComponent <BHingedConstraint>() != null)
{
if (rigidNode.GetSkeletalJoint().cDriver.port1 == i + 1)
{
float maxSpeed = 0f;
float impulse = 0f;
float friction = 0f;
if (rigidNode.GetSkeletalJoint().cDriver.InputGear != 0 && rigidNode.GetSkeletalJoint().cDriver.OutputGear != 0)
{
impulse *= Convert.ToSingle(rigidNode.GetSkeletalJoint().cDriver.InputGear / rigidNode.GetSkeletalJoint().cDriver.OutputGear);
}
if (rigidNode.HasDriverMeta <WheelDriverMeta>())
{
maxSpeed = WheelMaxSpeed;
impulse = WheelMotorImpulse;
friction = WheelCoastFriction;
}
else
{
maxSpeed = HingeMaxSpeed;
impulse = HingeMotorImpulse;
friction = HingeCostFriction;
}
BHingedConstraint hingedConstraint = rigidNode.MainObject.GetComponent <BHingedConstraint>();
hingedConstraint.enableMotor = true;
hingedConstraint.targetMotorAngularVelocity = pwm[i] > 0f ? maxSpeed : pwm[i] < 0f ? -maxSpeed : 0f;
hingedConstraint.maxMotorImpulse = rigidNode.GetSkeletalJoint().cDriver.hasBrake ? HingeMotorImpulse : pwm[i] == 0f ? friction : Mathf.Abs(pwm[i] * impulse);
}
}
else if (rigidNode.GetSkeletalJoint().cDriver.GetDriveType().IsElevator())
{
if (rigidNode.GetSkeletalJoint().cDriver.port1 == i + 1 && rigidNode.HasDriverMeta <ElevatorDriverMeta>())
{
BSliderConstraint bSliderConstraint = rigidNode.MainObject.GetComponent <BSliderConstraint>();
SliderConstraint sc = (SliderConstraint)bSliderConstraint.GetConstraint();
sc.PoweredLinearMotor = true;
sc.MaxLinearMotorForce = MaxSliderForce;
sc.TargetLinearMotorVelocity = pwm[i] * MaxSliderSpeed;
}
}
}
}
}
}
protected override void OnInitializePhysics()
{
SetupEmptyDynamicsWorld();
CollisionShape groundShape = new BoxShape(50, 1, 50);
//CollisionShape groundShape = new StaticPlaneShape(Vector3.UnitY, 40);
CollisionShapes.Add(groundShape);
RigidBody body = LocalCreateRigidBody(0, Matrix.Translation(0, -16, 0), groundShape);
body.UserObject = "Ground";
CollisionShape shape = new BoxShape(new Vector3(CubeHalfExtents));
CollisionShapes.Add(shape);
const float THETA = (float)Math.PI / 4.0f;
float L_1 = 2 - (float)Math.Tan(THETA);
float L_2 = 1 / (float)Math.Cos(THETA);
float RATIO = L_2 / L_1;
RigidBody bodyA;
RigidBody bodyB;
CollisionShape cylA = new CylinderShape(0.2f, 0.25f, 0.2f);
CollisionShape cylB = new CylinderShape(L_1, 0.025f, L_1);
CompoundShape cyl0 = new CompoundShape();
cyl0.AddChildShape(Matrix.Identity, cylA);
cyl0.AddChildShape(Matrix.Identity, cylB);
float mass = 6.28f;
Vector3 localInertia;
cyl0.CalculateLocalInertia(mass, out localInertia);
RigidBodyConstructionInfo ci = new RigidBodyConstructionInfo(mass, null, cyl0, localInertia);
ci.StartWorldTransform = Matrix.Translation(-8, 1, -8);
body = new RigidBody(ci); //1,0,cyl0,localInertia);
World.AddRigidBody(body);
body.LinearFactor = Vector3.Zero;
body.AngularFactor = new Vector3(0, 1, 0);
bodyA = body;
cylA = new CylinderShape(0.2f, 0.26f, 0.2f);
cylB = new CylinderShape(L_2, 0.025f, L_2);
cyl0 = new CompoundShape();
cyl0.AddChildShape(Matrix.Identity, cylA);
cyl0.AddChildShape(Matrix.Identity, cylB);
mass = 6.28f;
cyl0.CalculateLocalInertia(mass, out localInertia);
ci = new RigidBodyConstructionInfo(mass, null, cyl0, localInertia);
Quaternion orn = Quaternion.RotationAxis(new Vector3(0, 0, 1), -THETA);
ci.StartWorldTransform = Matrix.RotationQuaternion(orn) * Matrix.Translation(-10, 2, -8);
body = new RigidBody(ci);//1,0,cyl0,localInertia);
body.LinearFactor = Vector3.Zero;
HingeConstraint hinge = new HingeConstraint(body, Vector3.Zero, new Vector3(0, 1, 0), true);
World.AddConstraint(hinge);
bodyB = body;
body.AngularVelocity = new Vector3(0, 3, 0);
World.AddRigidBody(body);
Vector3 axisA = new Vector3(0, 1, 0);
Vector3 axisB = new Vector3(0, 1, 0);
orn = Quaternion.RotationAxis(new Vector3(0, 0, 1), -THETA);
Matrix mat = Matrix.RotationQuaternion(orn);
axisB = new Vector3(mat.M21, mat.M22, mat.M23);
GearConstraint gear = new GearConstraint(bodyA, bodyB, axisA, axisB, RATIO);
World.AddConstraint(gear, true);
mass = 1.0f;
RigidBody body0 = LocalCreateRigidBody(mass, Matrix.Translation(0, 20, 0), shape);
RigidBody body1 = null;//LocalCreateRigidBody(mass, Matrix.Translation(2*CUBE_HALF_EXTENTS,20,0), shape);
//RigidBody body1 = LocalCreateRigidBody(0, Matrix.Translation(2*CUBE_HALF_EXTENTS,20,0), null);
//body1.ActivationState = ActivationState.DisableDeactivation;
//body1.SetDamping(0.3f, 0.3f);
Vector3 pivotInA = new Vector3(CubeHalfExtents, -CubeHalfExtents, -CubeHalfExtents);
Vector3 axisInA = new Vector3(0, 0, 1);
Vector3 pivotInB;
if (body1 != null)
{
Matrix transform = Matrix.Invert(body1.CenterOfMassTransform) * body0.CenterOfMassTransform;
pivotInB = Vector3.TransformCoordinate(pivotInA, transform);
}
else
{
pivotInB = pivotInA;
}
Vector3 axisInB;
if (body1 != null)
{
Matrix transform = Matrix.Invert(body1.CenterOfMassTransform) * body1.CenterOfMassTransform;
axisInB = Vector3.TransformCoordinate(axisInA, transform);
}
else
{
axisInB = Vector3.TransformCoordinate(axisInA, body0.CenterOfMassTransform);
}
#if P2P
{
TypedConstraint p2p = new Point2PointConstraint(body0, pivotInA);
//TypedConstraint p2p = new Point2PointConstraint(body0, body1, pivotInA, pivotInB);
//TypedConstraint hinge = new HingeConstraint(body0, body1, pivotInA, pivotInB, axisInA, axisInB);
World.AddConstraint(p2p);
p2p.DebugDrawSize = 5;
}
#else
{
hinge = new HingeConstraint(body0, pivotInA, axisInA);
//use zero targetVelocity and a small maxMotorImpulse to simulate joint friction
//float targetVelocity = 0.f;
//float maxMotorImpulse = 0.01;
const float targetVelocity = 1.0f;
const float maxMotorImpulse = 1.0f;
hinge.EnableAngularMotor(true, targetVelocity, maxMotorImpulse);
World.AddConstraint(hinge);
hinge.DebugDrawSize = 5;
}
#endif
RigidBody pRbA1 = LocalCreateRigidBody(mass, Matrix.Translation(-20, 0, 30), shape);
//RigidBody pRbA1 = LocalCreateRigidBody(0.0f, Matrix.Translation(-20, 0, 30), shape);
pRbA1.ActivationState = ActivationState.DisableDeactivation;
// add dynamic rigid body B1
RigidBody pRbB1 = LocalCreateRigidBody(mass, Matrix.Translation(-20, 0, 30), shape);
//RigidBody pRbB1 = LocalCreateRigidBody(0.0f, Matrix.Translation(-20, 0, 30), shape);
pRbB1.ActivationState = ActivationState.DisableDeactivation;
// create slider constraint between A1 and B1 and add it to world
SliderConstraint spSlider1 = new SliderConstraint(pRbA1, pRbB1, Matrix.Identity, Matrix.Identity, true);
//spSlider1 = new SliderConstraint(pRbA1, pRbB1, Matrix.Identity, Matrix.Identity, false);
spSlider1.LowerLinearLimit = -15.0f;
spSlider1.UpperLinearLimit = -5.0f;
spSlider1.LowerLinearLimit = 5.0f;
spSlider1.UpperLinearLimit = 15.0f;
spSlider1.LowerLinearLimit = -10.0f;
spSlider1.UpperLinearLimit = -10.0f;
spSlider1.LowerAngularLimit = -(float)Math.PI / 3.0f;
spSlider1.UpperAngularLimit = (float)Math.PI / 3.0f;
World.AddConstraint(spSlider1, true);
spSlider1.DebugDrawSize = 5.0f;
//create a slider, using the generic D6 constraint
Vector3 sliderWorldPos = new Vector3(0, 10, 0);
Vector3 sliderAxis = Vector3.UnitX;
const float angle = 0; //SIMD_RADS_PER_DEG * 10.f;
Matrix trans = Matrix.RotationAxis(sliderAxis, angle) * Matrix.Translation(sliderWorldPos);
d6body0 = LocalCreateRigidBody(mass, trans, shape);
d6body0.ActivationState = ActivationState.DisableDeactivation;
RigidBody fixedBody1 = LocalCreateRigidBody(0, trans, null);
World.AddRigidBody(fixedBody1);
Matrix frameInA = Matrix.Translation(0, 5, 0);
Matrix frameInB = Matrix.Translation(0, 5, 0);
//bool useLinearReferenceFrameA = false;//use fixed frame B for linear llimits
const bool useLinearReferenceFrameA = true; //use fixed frame A for linear llimits
spSlider6Dof = new Generic6DofConstraint(fixedBody1, d6body0, frameInA, frameInB, useLinearReferenceFrameA)
{
LinearLowerLimit = lowerSliderLimit,
LinearUpperLimit = hiSliderLimit,
//range should be small, otherwise singularities will 'explode' the constraint
//AngularLowerLimit = new Vector3(-1.5f,0,0),
//AngularUpperLimit = new Vector3(1.5f,0,0),
//AngularLowerLimit = new Vector3(0,0,0),
//AngularUpperLimit = new Vector3(0,0,0),
AngularLowerLimit = new Vector3((float)-Math.PI, 0, 0),
AngularUpperLimit = new Vector3(1.5f, 0, 0)
};
//spSlider6Dof.TranslationalLimitMotor.EnableMotor[0] = true;
spSlider6Dof.TranslationalLimitMotor.TargetVelocity = new Vector3(-5.0f, 0, 0);
spSlider6Dof.TranslationalLimitMotor.MaxMotorForce = new Vector3(0.1f, 0, 0);
World.AddConstraint(spSlider6Dof);
spSlider6Dof.DebugDrawSize = 5;
// create a door using hinge constraint attached to the world
CollisionShape pDoorShape = new BoxShape(2.0f, 5.0f, 0.2f);
CollisionShapes.Add(pDoorShape);
RigidBody pDoorBody = LocalCreateRigidBody(1.0f, Matrix.Translation(-5.0f, -2.0f, 0.0f), pDoorShape);
pDoorBody.ActivationState = ActivationState.DisableDeactivation;
Vector3 btPivotA = new Vector3(10.0f + 2.1f, -2.0f, 0.0f); // right next to the door slightly outside
Vector3 btAxisA = Vector3.UnitY; // pointing upwards, aka Y-axis
spDoorHinge = new HingeConstraint(pDoorBody, btPivotA, btAxisA);
//spDoorHinge.SetLimit(0.0f, (float)Math.PI / 2);
// test problem values
//spDoorHinge.SetLimit(-(float)Math.PI, (float)Math.PI * 0.8f);
//spDoorHinge.SetLimit(1, -1);
//spDoorHinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI);
//spDoorHinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI, 0.9f, 0.3f, 0.0f);
//spDoorHinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI, 0.9f, 0.01f, 0.0f); // "sticky limits"
spDoorHinge.SetLimit(-(float)Math.PI * 0.25f, (float)Math.PI * 0.25f);
//spDoorHinge.SetLimit(0, 0);
World.AddConstraint(spDoorHinge);
spDoorHinge.DebugDrawSize = 5;
RigidBody pDropBody = LocalCreateRigidBody(10.0f, Matrix.Translation(-5.0f, 2.0f, 0.0f), shape);
// create a generic 6DOF constraint
//RigidBody pBodyA = LocalCreateRigidBody(mass, Matrix.Translation(10.0f, 6.0f, 0), shape);
RigidBody pBodyA = LocalCreateRigidBody(0, Matrix.Translation(10, 6, 0), shape);
//RigidBody pBodyA = LocalCreateRigidBody(0, Matrix.Translation(10, 6, 0), null);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
RigidBody pBodyB = LocalCreateRigidBody(mass, Matrix.Translation(0, 6, 0), shape);
//RigidBody pBodyB = LocalCreateRigidBody(0, Matrix.Translation(0, 6, 0), shape);
pBodyB.ActivationState = ActivationState.DisableDeactivation;
frameInA = Matrix.Translation(-5, 0, 0);
frameInB = Matrix.Translation(5, 0, 0);
Generic6DofConstraint pGen6DOF = new Generic6DofConstraint(pBodyA, pBodyB, frameInA, frameInB, true);
//Generic6DofConstraint pGen6DOF = new Generic6DofConstraint(pBodyA, pBodyB, frameInA, frameInB, false);
pGen6DOF.LinearLowerLimit = new Vector3(-10, -2, -1);
pGen6DOF.LinearUpperLimit = new Vector3(10, 2, 1);
//pGen6DOF.LinearLowerLimit = new Vector3(-10, 0, 0);
//pGen6DOF.LinearUpperLimit = new Vector3(10, 0, 0);
//pGen6DOF.LinearLowerLimit = new Vector3(0, 0, 0);
//pGen6DOF.LinearUpperLimit = new Vector3(0, 0, 0);
//pGen6DOF.TranslationalLimitMotor.EnableMotor[0] = true;
//pGen6DOF.TranslationalLimitMotor.TargetVelocity = new Vector3(5, 0, 0);
//pGen6DOF.TranslationalLimitMotor.MaxMotorForce = new Vector3(0.1f, 0, 0);
//pGen6DOF.AngularLowerLimit = new Vector3(0, (float)Math.PI * 0.9f, 0);
//pGen6DOF.AngularUpperLimit = new Vector3(0, -(float)Math.PI * 0.9f, 0);
//pGen6DOF.AngularLowerLimit = new Vector3(0, 0, -(float)Math.PI);
//pGen6DOF.AngularUpperLimit = new Vector3(0, 0, (float)Math.PI);
pGen6DOF.AngularLowerLimit = new Vector3(-(float)Math.PI / 4, -0.75f, -(float)Math.PI * 0.4f);
pGen6DOF.AngularUpperLimit = new Vector3((float)Math.PI / 4, 0.75f, (float)Math.PI * 0.4f);
//pGen6DOF.AngularLowerLimit = new Vector3(0, -0.75f, (float)Math.PI * 0.8f);
//pGen6DOF.AngularUpperLimit = new Vector3(0, 0.75f, -(float)Math.PI * 0.8f);
//pGen6DOF.AngularLowerLimit = new Vector3(0, -(float)Math.PI * 0.8f, (float)Math.PI * 1.98f);
//pGen6DOF.AngularUpperLimit = new Vector3(0, (float)Math.PI * 0.8f, -(float)Math.PI * 1.98f);
//pGen6DOF.AngularLowerLimit = new Vector3(-0.75f, -0.5f, -0.5f);
//pGen6DOF.AngularUpperLimit = new Vector3(0.75f, 0.5f, 0.5f);
//pGen6DOF.AngularLowerLimit = new Vector3(-0.75f, 0, 0);
//pGen6DOF.AngularUpperLimit = new Vector3(0.75f, 0, 0);
//pGen6DOF.AngularLowerLimit = new Vector3(0, -0.7f, 0);
//pGen6DOF.AngularUpperLimit = new Vector3(0, 0.7f, 0);
//pGen6DOF.AngularLowerLimit = new Vector3(-1, 0, 0);
//pGen6DOF.AngularUpperLimit = new Vector3(1, 0, 0);
// create a ConeTwist constraint
pBodyA = LocalCreateRigidBody(1.0f, Matrix.Translation(-10, 5, 0), shape);
//pBodyA = LocalCreateRigidBody(0, Matrix.Translation(-10, 5, 0), shape);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
pBodyB = LocalCreateRigidBody(0, Matrix.Translation(-10, -5, 0), shape);
//pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(-10, -5, 0), shape);
frameInA = Matrix.RotationYawPitchRoll(0, 0, (float)Math.PI / 2);
frameInA *= Matrix.Translation(0, -5, 0);
frameInB = Matrix.RotationYawPitchRoll(0, 0, (float)Math.PI / 2);
frameInB *= Matrix.Translation(0, 5, 0);
coneTwist = new ConeTwistConstraint(pBodyA, pBodyB, frameInA, frameInB);
//coneTwist.SetLimit((float)Math.PI / 4, (float)Math.PI / 4, (float)Math.PI * 0.8f);
//coneTwist.SetLimit((((float)Math.PI / 4) * 0.6f), (float)Math.PI / 4, (float)Math.PI * 0.8f, 1.0f); // soft limit == hard limit
coneTwist.SetLimit((((float)Math.PI / 4) * 0.6f), (float)Math.PI / 4, (float)Math.PI * 0.8f, 0.5f);
World.AddConstraint(coneTwist, true);
coneTwist.DebugDrawSize = 5;
// Hinge connected to the world, with motor (to hinge motor with new and old constraint solver)
RigidBody pBody = LocalCreateRigidBody(1.0f, Matrix.Identity, shape);
pBody.ActivationState = ActivationState.DisableDeactivation;
Vector3 pivotA = new Vector3(10.0f, 0.0f, 0.0f);
btAxisA = new Vector3(0.0f, 0.0f, 1.0f);
HingeConstraint pHinge = new HingeConstraint(pBody, pivotA, btAxisA);
//pHinge.EnableAngularMotor(true, -1.0f, 0.165f); // use for the old solver
pHinge.EnableAngularMotor(true, -1.0f, 1.65f); // use for the new SIMD solver
World.AddConstraint(pHinge);
pHinge.DebugDrawSize = 5;
// create a universal joint using generic 6DOF constraint
// create two rigid bodies
// static bodyA (parent) on top:
pBodyA = LocalCreateRigidBody(0, Matrix.Translation(20, 4, 0), shape);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
// dynamic bodyB (child) below it :
pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(20, 0, 0), shape);
pBodyB.ActivationState = ActivationState.DisableDeactivation;
// add some (arbitrary) data to build constraint frames
Vector3 parentAxis = new Vector3(1, 0, 0);
Vector3 childAxis = new Vector3(0, 0, 1);
Vector3 anchor = new Vector3(20, 2, 0);
UniversalConstraint pUniv = new UniversalConstraint(pBodyA, pBodyB, anchor, parentAxis, childAxis);
pUniv.SetLowerLimit(-(float)Math.PI / 4, -(float)Math.PI / 4);
pUniv.SetUpperLimit((float)Math.PI / 4, (float)Math.PI / 4);
// add constraint to world
World.AddConstraint(pUniv, true);
// draw constraint frames and limits for debugging
pUniv.DebugDrawSize = 5;
World.AddConstraint(pGen6DOF, true);
pGen6DOF.DebugDrawSize = 5;
// create a generic 6DOF constraint with springs
pBodyA = LocalCreateRigidBody(0, Matrix.Translation(-20, 16, 0), shape);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(-10, 16, 0), shape);
pBodyB.ActivationState = ActivationState.DisableDeactivation;
frameInA = Matrix.Translation(10, 0, 0);
frameInB = Matrix.Identity;
Generic6DofSpringConstraint pGen6DOFSpring = new Generic6DofSpringConstraint(pBodyA, pBodyB, frameInA, frameInB, true)
{
LinearUpperLimit = new Vector3(5, 0, 0),
LinearLowerLimit = new Vector3(-5, 0, 0),
AngularLowerLimit = new Vector3(0, 0, -1.5f),
AngularUpperLimit = new Vector3(0, 0, 1.5f),
DebugDrawSize = 5
};
World.AddConstraint(pGen6DOFSpring, true);
pGen6DOFSpring.EnableSpring(0, true);
pGen6DOFSpring.SetStiffness(0, 39.478f);
pGen6DOFSpring.SetDamping(0, 0.5f);
pGen6DOFSpring.EnableSpring(5, true);
pGen6DOFSpring.SetStiffness(5, 39.478f);
pGen6DOFSpring.SetDamping(0, 0.3f);
pGen6DOFSpring.SetEquilibriumPoint();
// create a Hinge2 joint
// create two rigid bodies
// static bodyA (parent) on top:
pBodyA = LocalCreateRigidBody(0, Matrix.Translation(-20, 4, 0), shape);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
// dynamic bodyB (child) below it :
pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(-20, 0, 0), shape);
pBodyB.ActivationState = ActivationState.DisableDeactivation;
// add some data to build constraint frames
parentAxis = new Vector3(0, 1, 0);
childAxis = new Vector3(1, 0, 0);
anchor = new Vector3(-20, 0, 0);
Hinge2Constraint pHinge2 = new Hinge2Constraint(pBodyA, pBodyB, anchor, parentAxis, childAxis);
pHinge2.SetLowerLimit(-(float)Math.PI / 4);
pHinge2.SetUpperLimit((float)Math.PI / 4);
// add constraint to world
World.AddConstraint(pHinge2, true);
// draw constraint frames and limits for debugging
pHinge2.DebugDrawSize = 5;
// create a Hinge joint between two dynamic bodies
// create two rigid bodies
// static bodyA (parent) on top:
pBodyA = LocalCreateRigidBody(1.0f, Matrix.Translation(-20, -2, 0), shape);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
// dynamic bodyB:
pBodyB = LocalCreateRigidBody(10.0f, Matrix.Translation(-30, -2, 0), shape);
pBodyB.ActivationState = ActivationState.DisableDeactivation;
// add some data to build constraint frames
axisA = new Vector3(0, 1, 0);
axisB = new Vector3(0, 1, 0);
Vector3 pivotA2 = new Vector3(-5, 0, 0);
Vector3 pivotB = new Vector3(5, 0, 0);
spHingeDynAB = new HingeConstraint(pBodyA, pBodyB, pivotA2, pivotB, axisA, axisB);
spHingeDynAB.SetLimit(-(float)Math.PI / 4, (float)Math.PI / 4);
// add constraint to world
World.AddConstraint(spHingeDynAB, true);
// draw constraint frames and limits for debugging
spHingeDynAB.DebugDrawSize = 5;
}
public static void UpdateAllMotors(RigidNode_Base skeleton, UnityPacket.OutputStatePacket.DIOModule[] dioModules, int controlIndex, bool mecanum)
{
bool IsMecanum = mecanum;
int reverse = -1;
float[] pwm;
float[] can;
if (dioModules[0] != null)
{
pwm = dioModules[0].pwmValues;
can = dioModules[0].canValues;
}
else
{
pwm = new float[10];
can = new float[10];
}
if (IsMecanum)
{
#region Mecanum Drive
pwm[(int)MecanumPorts.FRONT_RIGHT] +=
(InputControl.GetAxis(Controls.axes[controlIndex].vertical) * -SPEED_ARROW_PWM) +
(InputControl.GetAxis(Controls.axes[controlIndex].horizontal) * -SPEED_ARROW_PWM) +
(InputControl.GetAxis(Controls.axes[controlIndex].pwm2Axes) * -SPEED_ARROW_PWM);
pwm[(int)MecanumPorts.FRONT_LEFT] +=
(InputControl.GetAxis(Controls.axes[controlIndex].vertical) * SPEED_ARROW_PWM) +
(InputControl.GetAxis(Controls.axes[controlIndex].horizontal) * SPEED_ARROW_PWM) +
(InputControl.GetAxis(Controls.axes[controlIndex].pwm2Axes) * -SPEED_ARROW_PWM);
//For some reason, giving the back wheels 0.25 power instead of 0.5 works for strafing
pwm[(int)MecanumPorts.BACK_RIGHT] +=
(InputControl.GetAxis(Controls.axes[controlIndex].vertical) * -SPEED_ARROW_PWM) +
(InputControl.GetAxis(Controls.axes[controlIndex].horizontal) * -SPEED_ARROW_PWM) +
(InputControl.GetAxis(Controls.axes[controlIndex].pwm2Axes) * 0.25f);
pwm[(int)MecanumPorts.BACK_LEFT] +=
(InputControl.GetAxis(Controls.axes[controlIndex].vertical) * SPEED_ARROW_PWM) +
(InputControl.GetAxis(Controls.axes[controlIndex].horizontal) * SPEED_ARROW_PWM) +
(InputControl.GetAxis(Controls.axes[controlIndex].pwm2Axes) * 0.25f);
pwm[4] +=
(InputControl.GetAxis(Controls.axes[controlIndex].pwm4Axes) * SPEED_ARROW_PWM);
pwm[5] +=
(InputControl.GetAxis(Controls.axes[controlIndex].pwm5Axes) * SPEED_ARROW_PWM);
pwm[6] +=
(InputControl.GetAxis(Controls.axes[controlIndex].pwm6Axes) * SPEED_ARROW_PWM);
#endregion
}
if (Controls.TankDriveEnabled)
{
#region Tank Drive
//pwm[0] +=
// (InputControl.GetButton(Controls.buttons[controlIndex].tankFrontLeft) ? SPEED_ARROW_PWM : 0.0f) +
// (InputControl.GetButton(Controls.buttons[controlIndex].tankBackLeft) ? -SPEED_ARROW_PWM : 0.0f);
//pwm[1] +=
// (InputControl.GetButton(Controls.buttons[controlIndex].tankFrontRight) ? -SPEED_ARROW_PWM : 0.0f) +
// (InputControl.GetButton(Controls.buttons[controlIndex].tankBackRight) ? SPEED_ARROW_PWM : 0.0f);
pwm[0] +=
(InputControl.GetAxis(Controls.axes[controlIndex].tankRightAxes) * SPEED_ARROW_PWM);
pwm[1] +=
(InputControl.GetAxis(Controls.axes[controlIndex].tankLeftAxes) * SPEED_ARROW_PWM);
pwm[2] +=
(InputControl.GetAxis(Controls.axes[controlIndex].pwm2Axes) * SPEED_ARROW_PWM);
pwm[3] +=
(InputControl.GetAxis(Controls.axes[controlIndex].pwm3Axes) * SPEED_ARROW_PWM);
pwm[4] +=
(InputControl.GetAxis(Controls.axes[controlIndex].pwm4Axes) * SPEED_ARROW_PWM);
pwm[5] +=
(InputControl.GetAxis(Controls.axes[controlIndex].pwm5Axes) * SPEED_ARROW_PWM);
pwm[6] +=
(InputControl.GetAxis(Controls.axes[controlIndex].pwm6Axes) * SPEED_ARROW_PWM);
#endregion
}
else
{
#region Arcade Drive
//pwm[0] +=
// (InputControl.GetButton(Controls.buttons[controlIndex].forward) ? SPEED_ARROW_PWM : 0.0f) +
// (InputControl.GetButton(Controls.buttons[controlIndex].backward) ? -SPEED_ARROW_PWM : 0.0f) +
// (InputControl.GetButton(Controls.buttons[controlIndex].left) ? -SPEED_ARROW_PWM : 0.0f) +
// (InputControl.GetButton(Controls.buttons[controlIndex].right) ? SPEED_ARROW_PWM : 0.0f);
//pwm[1] +=
// (InputControl.GetButton(Controls.buttons[controlIndex].forward) ? -SPEED_ARROW_PWM : 0.0f) +
// (InputControl.GetButton(Controls.buttons[controlIndex].backward) ? SPEED_ARROW_PWM : 0.0f) +
// (InputControl.GetButton(Controls.buttons[controlIndex].left) ? -SPEED_ARROW_PWM : 0.0f) +
// (InputControl.GetButton(Controls.buttons[controlIndex].right) ? SPEED_ARROW_PWM : 0.0f);
pwm[0] +=
(InputControl.GetAxis(Controls.axes[controlIndex].vertical) * -SPEED_ARROW_PWM) +
(InputControl.GetAxis(Controls.axes[controlIndex].horizontal) * SPEED_ARROW_PWM);
pwm[1] +=
(InputControl.GetAxis(Controls.axes[controlIndex].vertical) * SPEED_ARROW_PWM) +
(InputControl.GetAxis(Controls.axes[controlIndex].horizontal) * SPEED_ARROW_PWM);
pwm[2] +=
(InputControl.GetAxis(Controls.axes[controlIndex].pwm2Axes) * SPEED_ARROW_PWM);
pwm[3] +=
(InputControl.GetAxis(Controls.axes[controlIndex].pwm3Axes) * SPEED_ARROW_PWM);
pwm[4] +=
(InputControl.GetAxis(Controls.axes[controlIndex].pwm4Axes) * SPEED_ARROW_PWM);
pwm[5] +=
(InputControl.GetAxis(Controls.axes[controlIndex].pwm5Axes) * SPEED_ARROW_PWM);
pwm[6] +=
(InputControl.GetAxis(Controls.axes[controlIndex].pwm6Axes) * SPEED_ARROW_PWM);
#endregion
}
listOfSubNodes.Clear();
skeleton.ListAllNodes(listOfSubNodes);
for (int i = 0; i < pwm.Length; i++)
{
foreach (RigidNode_Base node in listOfSubNodes)
{
RigidNode rigidNode = (RigidNode)node;
if (pwm[i] != 0f)
{
BRigidBody rigidBody = rigidNode.MainObject.GetComponent <BRigidBody>();
if (rigidBody != null && !rigidBody.GetCollisionObject().IsActive)
{
rigidBody.GetCollisionObject().Activate();
}
}
BRaycastWheel raycastWheel = rigidNode.MainObject.GetComponent <BRaycastWheel>();
if (raycastWheel != null)
{
if (rigidNode.GetSkeletalJoint().cDriver.portA == i + 1)
{
raycastWheel.ApplyForce(pwm[i]);
}
}
if (rigidNode.GetSkeletalJoint() != null && rigidNode.GetSkeletalJoint().cDriver != null)
{
if (rigidNode.GetSkeletalJoint().cDriver.GetDriveType().IsMotor() && rigidNode.MainObject.GetComponent <BHingedConstraint>() != null)
{
if (rigidNode.GetSkeletalJoint().cDriver.portA == i + 1)
{
float maxSpeed = 0f;
float impulse = 0f;
float friction = 0f;
if (rigidNode.HasDriverMeta <WheelDriverMeta>())
{
maxSpeed = WHEEL_MAX_SPEED;
impulse = WHEEL_MOTOR_IMPULSE;
friction = WHEEL_COAST_FRICTION;
}
else
{
maxSpeed = HINGE_MAX_SPEED;
impulse = HINGE_MOTOR_IMPULSE;
friction = HINGE_COAST_FRICTION;
}
BHingedConstraint hingedConstraint = rigidNode.MainObject.GetComponent <BHingedConstraint>();
hingedConstraint.enableMotor = true;
hingedConstraint.targetMotorAngularVelocity = pwm[i] > 0f ? maxSpeed : pwm[i] < 0f ? -maxSpeed : 0f;
hingedConstraint.maxMotorImpulse = pwm[i] == 0f ? friction : Mathf.Abs(pwm[i] * impulse);
}
}
else if (rigidNode.GetSkeletalJoint().cDriver.GetDriveType().IsElevator())
{
if (rigidNode.GetSkeletalJoint().cDriver.portA == i + 1 && rigidNode.HasDriverMeta <ElevatorDriverMeta>())
{
BSliderConstraint bSliderConstraint = rigidNode.MainObject.GetComponent <BSliderConstraint>();
SliderConstraint sc = (SliderConstraint)bSliderConstraint.GetConstraint();
sc.PoweredLinearMotor = true;
sc.MaxLinearMotorForce = MAX_SLIDER_FORCE;
sc.TargetLinearMotorVelocity = pwm[i] * MAX_SLIDER_SPEED;
}
}
}
}
}
}
