protected internal override void OnApplyConstraint(UserJointBase joint)
{
Matrix3D m = Joint.BodyToWorldMatrix(joint.ParentBody, _parentDirection);
Matrix3D c = Joint.BodyToWorldMatrix(joint.ChildBody, this.Direction);
double sa = Vector3D.DotProduct(Vector3D.CrossProduct(Math3D.GetUpVector(ref c), Math3D.GetUpVector(ref m)), Math3D.GetFrontVector(ref c));
double ca = Vector3D.DotProduct(Math3D.GetUpVector(ref c), Math3D.GetUpVector(ref m));
double a = Math.Atan2(sa, ca);
joint.NewtonJoint.UserBilateralAddAngularRow((float)a, Math3D.GetFrontVector(ref m));
ApplyConstraintProperties(joint);
sa = Vector3D.DotProduct(Vector3D.CrossProduct(Math3D.GetRightVector(ref c), Math3D.GetRightVector(ref m)), Math3D.GetUpVector(ref c));
ca = Vector3D.DotProduct(Math3D.GetRightVector(ref c), Math3D.GetRightVector(ref m));
a = Math.Atan2(sa, ca);
joint.NewtonJoint.UserBilateralAddAngularRow((float)a, Math3D.GetUpVector(ref m));
ApplyConstraintProperties(joint);
sa = Vector3D.DotProduct(Vector3D.CrossProduct(Math3D.GetFrontVector(ref c), Math3D.GetFrontVector(ref m)), Math3D.GetRightVector(ref c));
ca = Vector3D.DotProduct(Math3D.GetFrontVector(ref c), Math3D.GetFrontVector(ref m));
a = Math.Atan2(sa, ca);
joint.NewtonJoint.UserBilateralAddAngularRow((float)a, Math3D.GetRightVector(ref m));
ApplyConstraintProperties(joint);
}
internal protected void UpdateForce(UserJointBase joint, int index)
{
if (_averages.Count > ForceAverageSampleCount)
{
_averages.RemoveAt(0);
}
var impulseForce = joint.NewtonJoint.UserBilateralGetRowForce(index);
_averages.Add(impulseForce);
ImpulseForce = impulseForce;
int count = _averages.Count;
if (count >= ForceAverageSampleCount)
{
double force = 0;
for (int i = 0; i < count; i++)
{
force += _averages[i];
}
Force = force / count;
}
}
public override void Initialise(UserJointBase joint)
{
base.Initialise(joint);
foreach (UserJointConstraint constraint in _children)
constraint.Initialise(joint);
}
protected internal override void ApplyConstraint(UserJointBase joint)
{
foreach (UserJointConstraint constraint in _children)
{
constraint.ApplyConstraint(joint);
}
}
protected internal override void ApplyConstraint(UserJointBase joint)
{
foreach (UserJointConstraint constraint in _children)
{
constraint.ApplyConstraint(joint);
}
}
protected internal override void OnApplyConstraint(UserJointBase joint)
{
Matrix3D m = Joint.BodyToWorldMatrix(joint.ParentBody, _parentDirection);
Matrix3D c = Joint.BodyToWorldMatrix(joint.ChildBody, this.Direction);
double sa = Vector3D.DotProduct(Vector3D.CrossProduct(Math3D.GetUpVector(ref c), Math3D.GetUpVector(ref m)), Math3D.GetFrontVector(ref c));
double ca = Vector3D.DotProduct(Math3D.GetUpVector(ref c), Math3D.GetUpVector(ref m));
double a = Math.Atan2(sa, ca);
joint.NewtonJoint.UserBilateralAddAngularRow((float)a, Math3D.GetFrontVector(ref m));
ApplyConstraintProperties(joint);
sa = Vector3D.DotProduct(Vector3D.CrossProduct(Math3D.GetRightVector(ref c), Math3D.GetRightVector(ref m)), Math3D.GetUpVector(ref c));
ca = Vector3D.DotProduct(Math3D.GetRightVector(ref c), Math3D.GetRightVector(ref m));
a = Math.Atan2(sa, ca);
joint.NewtonJoint.UserBilateralAddAngularRow((float)a, Math3D.GetUpVector(ref m));
ApplyConstraintProperties(joint);
sa = Vector3D.DotProduct(Vector3D.CrossProduct(Math3D.GetFrontVector(ref c), Math3D.GetFrontVector(ref m)), Math3D.GetRightVector(ref c));
ca = Vector3D.DotProduct(Math3D.GetFrontVector(ref c), Math3D.GetFrontVector(ref m));
a = Math.Atan2(sa, ca);
joint.NewtonJoint.UserBilateralAddAngularRow((float)a, Math3D.GetRightVector(ref m));
ApplyConstraintProperties(joint);
}
protected internal override void OnApplyConstraint(UserJointBase joint)
{
Matrix3D m = Joint.BodyToWorldMatrix(joint.ChildBody, this.PivotPoint, this.Direction);
Vector3D childPivotPoint = Math3D.GetOffset(ref m);
Vector3D parentPivotPoint = (Vector3D)Joint.BodyToWorld(joint.ParentBody, _parentPivotPoint);
joint.NewtonJoint.UserBilateralAddLinearRow(
childPivotPoint, parentPivotPoint,
Math3D.GetFrontVector(ref m)
);
ApplyConstraintProperties(joint);
joint.NewtonJoint.UserBilateralAddLinearRow(
childPivotPoint, parentPivotPoint,
Math3D.GetUpVector(ref m)
);
ApplyConstraintProperties(joint);
joint.NewtonJoint.UserBilateralAddLinearRow(
childPivotPoint, parentPivotPoint,
Math3D.GetRightVector(ref m)
);
ApplyConstraintProperties(joint);
}
protected internal override void OnApplyConstraint(UserJointBase joint)
{
Matrix3D m = Joint.BodyToWorldMatrix(joint.ChildBody, this.PivotPoint, this.Direction);
Vector3D childPivotPoint = Math3D.GetOffset(ref m);
Vector3D parentPivotPoint = (Vector3D)Joint.BodyToWorld(joint.ParentBody, _parentPivotPoint);
joint.NewtonJoint.UserBilateralAddLinearRow(
childPivotPoint, parentPivotPoint,
Math3D.GetFrontVector(ref m)
);
ApplyConstraintProperties(joint);
joint.NewtonJoint.UserBilateralAddLinearRow(
childPivotPoint, parentPivotPoint,
Math3D.GetUpVector(ref m)
);
ApplyConstraintProperties(joint);
joint.NewtonJoint.UserBilateralAddLinearRow(
childPivotPoint, parentPivotPoint,
Math3D.GetRightVector(ref m)
);
ApplyConstraintProperties(joint);
}
protected internal override void OnApplyConstraint(UserJointBase joint)
{
joint.NewtonJoint.UserBilateralAddAngularRow(
(float)RelativeAngleError,
Joint.BodyToWorld(joint.ChildBody, this.Axis)
);
ApplyConstraintProperties(joint);
}
protected internal override void OnApplyConstraint(UserJointBase joint)
{
joint.NewtonJoint.UserBilateralAddAngularRow(
(float)RelativeAngleError,
Joint.BodyToWorld(joint.ChildBody, this.Axis)
);
ApplyConstraintProperties(joint);
}
public override void Initialise(UserJointBase joint)
{
base.Initialise(joint);
foreach (UserJointConstraint constraint in _children)
{
constraint.Initialise(joint);
}
}
internal void Initialise(UserJointConstraint item)
{
UserJointBase joint = this.OwnerJoint;
if (joint != null)
{
if (joint.IsInitialised)
{
item.Initialise(joint);
}
}
}
protected virtual void ApplyConstraintProperties(UserJointBase joint)
{
CJointUserDefinedBilateral handle = joint.NewtonJoint;
object value = this.MinFriction;
if (value != null)
{
handle.UserBilateralRowMinimumFriction = (float)(double)value;
}
value = this.MaxFriction;
if (value != null)
{
handle.UserBilateralRowMaximumFriction = (float)(double)value;
}
value = this.Acceleration;
if (value != null)
{
handle.UserBilateralRowAcceleration = (float)(double)value;
}
double dvalue = this.Stiffness;
if (dvalue != STIFFNESS_DEFAULT)
{
handle.UserBilateralRowStiffness = (float)dvalue;
}
value = this.SpringStiffness;
object value2 = this.SpringDamper;
if ((value != null) && (value2 != null))
{
handle.UserBilateralSetRowSpringDamperAcceleration((float)(double)value, (float)(double)value2);
}
}
internal protected abstract void OnApplyConstraint(UserJointBase joint);
protected internal override void OnApplyConstraint(UserJointBase joint)
{
//
}
public virtual void Initialise(UserJointBase joint)
{
}
protected internal override void OnApplyConstraint(UserJointBase joint)
{
//
}
internal protected abstract void OnApplyConstraint(UserJointBase joint);
public override void Initialise(UserJointBase joint)
{
base.Initialise(joint);
Update();
}
protected virtual void ApplyConstraintProperties(UserJointBase joint)
{
CJointUserDefinedBilateral handle = joint.NewtonJoint;
object value = this.MinFriction;
if (value != null)
handle.UserBilateralRowMinimumFriction = (float)(double)value;
value = this.MaxFriction;
if (value != null)
handle.UserBilateralRowMaximumFriction = (float)(double)value;
value = this.Acceleration;
if (value != null)
handle.UserBilateralRowAcceleration = (float)(double)value;
double dvalue = this.Stiffness;
if (dvalue != STIFFNESS_DEFAULT)
handle.UserBilateralRowStiffness = (float)dvalue;
value = this.SpringStiffness;
object value2 = this.SpringDamper;
if ((value != null) && (value2 != null))
handle.UserBilateralSetRowSpringDamperAcceleration((float)(double)value, (float)(double)value2);
}
internal protected void UpdateForce(UserJointBase joint, int index)
{
if (_averages.Count > ForceAverageSampleCount)
_averages.RemoveAt(0);
var impulseForce = joint.NewtonJoint.UserBilateralGetRowForce(index);
_averages.Add(impulseForce);
ImpulseForce = impulseForce;
int count = _averages.Count;
if (count >= ForceAverageSampleCount)
{
double force = 0;
for (int i = 0; i < count; i++)
{
force += _averages[i];
}
Force = force / count;
}
}
internal protected virtual void ApplyConstraint(UserJointBase joint)
{
OnApplyConstraint(joint);
}
public virtual void Initialise(UserJointBase joint)
{
}
public override void Initialise(UserJointBase joint)
{
base.Initialise(joint);
Update();
}
internal protected virtual void ApplyConstraint(UserJointBase joint)
{
OnApplyConstraint(joint);
}
