private void UpdateObjectVelocity(
IShapeCommon simObj,
Vector3d linearComponent,
Vector3d angularComponent,
double x,
object sync)
{
if (!simObj.IsStatic)
{
Vector3d linearImpulse = x * linearComponent;
Vector3d angularImpuse = x * angularComponent;
Vector3d linearVelocity = linearImpulse *
simObj.MassInfo.InverseMass;
Vector3d angularVelocity = simObj.MassInfo.InverseInertiaTensor *
angularImpuse;
//Critical Section
lock (sync)
{
simObj.SetLinearVelocity(simObj.LinearVelocity + linearVelocity);
simObj.SetAngularVelocity(simObj.AngularVelocity + angularVelocity);
}
}
}
public JacobianConstraint(
IShapeCommon objectA,
IShapeCommon objectB,
int?contactReference,
Vector3d?linearComponentA,
Vector3d?linearComponentB,
Vector3d angularComponentA,
Vector3d angularComponentB,
ConstraintType type,
double constraintValue,
double jacobianVelocity,
double correctionValue,
double cfm,
double constraintLimit)
{
ObjectA = objectA;
ObjectB = objectB;
ContactReference = contactReference;
LinearComponentA = linearComponentA;
LinearComponentB = linearComponentB;
AngularComponentA = angularComponentA;
AngularComponentB = angularComponentB;
Type = type;
ConstraintValue = constraintValue;
JacobianVelocity = jacobianVelocity;
CorrectionValue = correctionValue;
CFM = cfm;
ConstraintLimit = constraintLimit;
}
public static JacobianConstraint GetDOF(
Vector3d angularComponentA,
Vector3d angularComponentB,
IShapeCommon simulationObjectA,
IShapeCommon simulationObjectB,
double constraintValue,
double correctionValue,
double cfm,
double constraintLimit,
ConstraintType type,
int?contactReference)
{
double jacobianVelocityValue = angularComponentA.Dot(simulationObjectA.AngularVelocity) +
angularComponentB.Dot(simulationObjectB.AngularVelocity);
return(new JacobianConstraint(
simulationObjectA,
simulationObjectB,
contactReference,
null,
null,
angularComponentA,
angularComponentB,
type,
constraintValue,
jacobianVelocityValue,
correctionValue,
cfm,
constraintLimit));
}
public JacobianConstraint(
IShapeCommon objectA,
IShapeCommon objectB,
Vector3d angularComponentA,
Vector3d angularComponentB,
ConstraintType type,
double constraintValue,
double jacobianVelocity,
double correctionValue,
double cfm,
double constraintLimit)
: this(objectA, objectB, null, null, null, angularComponentA, angularComponentB, type, constraintValue, jacobianVelocity, correctionValue, cfm, constraintLimit)
{
}
public JacobianConstraint(JacobianConstraint jc)
{
ObjectA = jc.ObjectA;
ObjectB = jc.ObjectB;
ContactReference = jc.ContactReference;
LinearComponentA = jc.LinearComponentA;
LinearComponentB = jc.LinearComponentB;
AngularComponentA = jc.AngularComponentA;
AngularComponentB = jc.AngularComponentB;
Type = jc.Type;
JacobianVelocity = jc.JacobianVelocity;
ConstraintValue = jc.ConstraintValue;
JacobianVelocity = jc.JacobianVelocity;
CorrectionValue = jc.CorrectionValue;
CFM = jc.CFM;
ConstraintLimit = jc.ConstraintLimit;
}
