本文整理汇总了C#中LimitState类的典型用法代码示例。如果您正苦于以下问题：C# LimitState类的具体用法？C# LimitState怎么用？C# LimitState使用的例子？那么恭喜您, 这里精选的类代码示例或许可以为您提供帮助。
LimitState类属于命名空间，在下文中一共展示了LimitState类的20个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于我们的系统推荐出更棒的C#代码示例。
示例1: RopeJoint
internal RopeJoint(IWorldPool worldPool, RopeJointDef def)
: base(worldPool, def)
{
m_localAnchorA.set(def.localAnchorA);
m_localAnchorB.set(def.localAnchorB);
m_maxLength = def.maxLength;
m_mass = 0.0f;
m_impulse = 0.0f;
m_state = LimitState.INACTIVE;
m_length = 0.0f;
}
开发者ID:Nomad1，项目名称:sharpbox2d，代码行数:13，代码来源:RopeJoint.cs
示例2: RopeJoint
public RopeJoint(Body bodyA, Body bodyB, Vector2 localAnchorA, Vector2 localAnchorB)
: base(bodyA, bodyB)
{
LocalAnchorA = localAnchorA;
LocalAnchorB = localAnchorB;
Vector2 d = WorldAnchorB - WorldAnchorA;
MaxLength = d.Length();
_mass = 0.0f;
_impulse = 0.0f;
_state = LimitState.Inactive;
_length = 0.0f;
}
开发者ID:dvgamer，项目名称:GhostLegend-XNA，代码行数:14，代码来源:RopeJoint.cs
示例3: PreProcess
/// <summary>
/// Prepares the constraint for iterative processing in the current frame.
/// </summary>
public override void PreProcess()
{
RigidBody a = BodyA, b = BodyB;
// get world points and normal
Vector3.Transform(ref _bodyPointA, ref a.World.Combined, out _worldOffsetA);
Vector3.Transform(ref _bodyPointB, ref b.World.Combined, out _worldOffsetB);
Vector3.Subtract(ref _worldOffsetA, ref _worldOffsetB, out _normal);
Vector3.Subtract(ref _worldOffsetA, ref a.World.Position, out _worldOffsetA);
Vector3.Subtract(ref _worldOffsetB, ref b.World.Position, out _worldOffsetB);
_distance = _normal.Length();
if (_distance < Constants.Epsilon)
_normal = Vector3.Zero;
else
Vector3.Divide(ref _normal, _distance, out _normal);
_mass = MassProperties.EffectiveMass(ref a.MassWorld, ref b.MassWorld, ref _worldOffsetA, ref _worldOffsetB, ref _normal);
// determine the constraint behavior for this frame
var prevState = _state;
if (Math.Abs(_maxDistance - _minDistance) < 2f * this.Manager.LinearErrorTolerance)
_state = LimitState.Equal;
else if (_distance <= _minDistance)
_state = LimitState.Min;
else if (_distance >= _maxDistance)
_state = LimitState.Max;
else
_state = LimitState.Between;
if (this.Manager.IsSolverWarmStarted && prevState == _state)
{
Vector3 impulse;
Vector3.Multiply(ref _impulse, this.Manager.TimeStep, out impulse);
b.ApplyImpulse(ref impulse, ref _worldOffsetB);
Vector3.Negate(ref impulse, out impulse);
a.ApplyImpulse(ref impulse, ref _worldOffsetA);
}
else
_impulse = Vector3.Zero;
_pImpulse = Vector3.Zero;
}
开发者ID:jdddog，项目名称:jengasimulatorp4p，代码行数:46，代码来源:DistanceConstraint.cs
示例4: PreProcess
/// <summary>
/// Prepares the constraint for iterative processing in the current frame.
/// </summary>
public override void PreProcess()
{
ComputeVitals();
LimitState prevX = _statePosX, prevY = _statePosY, prevZ = _statePosZ;
_statePosX = ComputeLinearLimitState((_limitedPosAxes & Axes.X) > 0, _positions.X, _minPos.X, _maxPos.X);
_statePosY = ComputeLinearLimitState((_limitedPosAxes & Axes.Y) > 0, _positions.Y, _minPos.Y, _maxPos.Y);
_statePosZ = ComputeLinearLimitState((_limitedPosAxes & Axes.Z) > 0, _positions.Z, _minPos.Z, _maxPos.Z);
_stateRotX = ComputeAngularLimitState((_limitedRotAxes & Axes.X) > 0, _angles.X, _minAngles.X, _maxAngles.X);
_stateRotY = ComputeAngularLimitState((_limitedRotAxes & Axes.Y) > 0, _angles.Y, _minAngles.Y, _maxAngles.Y);
_stateRotZ = ComputeAngularLimitState((_limitedRotAxes & Axes.Z) > 0, _angles.Z, _minAngles.Z, _maxAngles.Z);
DoLinearWarmStart(prevX, _statePosX, ref _impulsePos.X, ref _worldBasisB.X);
DoLinearWarmStart(prevY, _statePosY, ref _impulsePos.Y, ref _worldBasisB.Y);
DoLinearWarmStart(prevZ, _statePosZ, ref _impulsePos.Z, ref _worldBasisB.Z);
_impulseRot = Vector3.Zero;
_pImpulsePosX = _pImpulsePosY = _pImpulsePosZ = Vector3.Zero;
_pImpulseRotX = _pImpulseRotY = _pImpulseRotZ = Vector3.Zero;
}
开发者ID:jdddog，项目名称:jengasimulatorp4p，代码行数:24，代码来源:GenericConstraint.cs
示例5: LineJoint
internal LineJoint(LineJointDef def)
: base(def)
{
_localAnchor1 = def.localAnchor1;
_localAnchor2 = def.localAnchor2;
_localXAxis1 = def.localAxis1;
_localYAxis1 = MathUtils.Cross(1.0f, _localXAxis1);
_impulse = Vector2.Zero;
_motorMass = 0.0f;
_motorImpulse = 0.0f;
_lowerTranslation = def.lowerTranslation;
_upperTranslation = def.upperTranslation;
_maxMotorForce = def.maxMotorForce;
_motorSpeed = def.motorSpeed;
_enableLimit = def.enableLimit;
_enableMotor = def.enableMotor;
_limitState = LimitState.Inactive;
_axis = Vector2.Zero;
_perp = Vector2.Zero;
}
开发者ID:Nukepayload2，项目名称:Box2D，代码行数:23，代码来源:LineJoint.cs
示例6: InitVelocityConstraints
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body b1 = _bodyA;
Body b2 = _bodyB;
_localCenter1 = b1.GetLocalCenter();
_localCenter2 = b2.GetLocalCenter();
XForm xf1, xf2;
b1.GetXForm(out xf1);
b2.GetXForm(out xf2);
// Compute the effective masses.
Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - _localCenter1);
Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - _localCenter2);
Vector2 d = b2._sweep.c + r2 - b1._sweep.c - r1;
_invMass1 = b1._invMass;
_invI1 = b1._invI;
_invMass2 = b2._invMass;
_invI2 = b2._invI;
// Compute motor Jacobian and effective mass.
{
_axis = MathUtils.Multiply(ref xf1.R, _localXAxis1);
_a1 = MathUtils.Cross(d + r1, _axis);
_a2 = MathUtils.Cross(r2, _axis);
_motorMass = _invMass1 + _invMass2 + _invI1 * _a1 * _a1 + _invI2 * _a2 * _a2;
Debug.Assert(_motorMass > Settings.b2_FLT_EPSILON);
_motorMass = 1.0f / _motorMass;
}
// Prismatic raint.
{
_perp = MathUtils.Multiply(ref xf1.R, _localYAxis1);
_s1 = MathUtils.Cross(d + r1, _perp);
_s2 = MathUtils.Cross(r2, _perp);
float m1 = _invMass1, m2 = _invMass2;
float i1 = _invI1, i2 = _invI2;
float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float k12 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
float k22 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;
_K.col1 = new Vector2(k11, k12);
_K.col2 = new Vector2(k12, k22);
}
// Compute motor and limit terms.
if (_enableLimit)
{
float jointTranslation = Vector2.Dot(_axis, d);
if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.b2_linearSlop)
{
_limitState = LimitState.Equal;
}
else if (jointTranslation <= _lowerTranslation)
{
if (_limitState != LimitState.AtLower)
{
_limitState = LimitState.AtLower;
_impulse.Y = 0.0f;
}
}
else if (jointTranslation >= _upperTranslation)
{
if (_limitState != LimitState.AtUpper)
{
_limitState = LimitState.AtUpper;
_impulse.Y = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
_impulse.Y = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
}
if (_enableMotor == false)
{
_motorImpulse = 0.0f;
}
if (step.warmStarting)
{
// Account for variable time step.
_impulse *= step.dtRatio;
_motorImpulse *= step.dtRatio;
Vector2 P = _impulse.X * _perp + (_motorImpulse + _impulse.Y) * _axis;
float L1 = _impulse.X * _s1 + (_motorImpulse + _impulse.Y) * _a1;
float L2 = _impulse.X * _s2 + (_motorImpulse + _impulse.Y) * _a2;
//.........这里部分代码省略.........
开发者ID:Nukepayload2，项目名称:Box2D，代码行数:101，代码来源:LineJoint.cs
示例7: InitVelocityConstraints
internal override void InitVelocityConstraints(TimeStep step)
{
Body b1 = _body1;
Body b2 = _body2;
// You cannot create a prismatic joint between bodies that
// both have fixed rotation.
Box2DXDebug.Assert(b1._invI > 0.0f || b2._invI > 0.0f);
_localCenter1 = b1.GetLocalCenter();
_localCenter2 = b2.GetLocalCenter();
Transform xf1 = b1.GetTransform();
Transform xf2 = b2.GetTransform();
// Compute the effective masses.
Vector2 r1 = xf1.TransformDirection(_localAnchor1 - _localCenter1);
Vector2 r2 = xf2.TransformDirection(_localAnchor2 - _localCenter2);
Vector2 d = b2._sweep.C + r2 - b1._sweep.C - r1;
_invMass1 = b1._invMass;
_invI1 = b1._invI;
_invMass2 = b2._invMass;
_invI2 = b2._invI;
// Compute motor Jacobian and effective mass.
{
_axis = xf1.TransformDirection(_localXAxis1);
_a1 = (d + r1).Cross(_axis);
_a2 = r2.Cross(_axis);
_motorMass = _invMass1 + _invMass2 + _invI1 * _a1 * _a1 + _invI2 * _a2 * _a2;
Box2DXDebug.Assert(_motorMass > Settings.FLT_EPSILON);
_motorMass = 1.0f / _motorMass;
}
// Prismatic constraint.
{
_perp = xf1.TransformDirection(_localYAxis1);
_s1 = (d + r1).Cross(_perp);
_s2 = r2.Cross(_perp);
float m1 = _invMass1, m2 = _invMass2;
float i1 = _invI1, i2 = _invI2;
float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float k12 = i1 * _s1 + i2 * _s2;
float k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
float k22 = i1 + i2;
float k23 = i1 * _a1 + i2 * _a2;
float k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;
_K.Col1 = new Vector3(k11, k12, k13);
_K.Col2 = new Vector3(k12, k22, k23);
_K.Col3 = new Vector3(k13, k23, k33);
}
// Compute motor and limit terms.
if (_enableLimit)
{
float jointTranslation = Vector2.Dot(_axis, d);
if (Box2DX.Common.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
_limitState = LimitState.EqualLimits;
}
else if (jointTranslation <= _lowerTranslation)
{
if (_limitState != LimitState.AtLowerLimit)
{
_limitState = LimitState.AtLowerLimit;
_impulse.z = 0.0f;
}
}
else if (jointTranslation >= _upperTranslation)
{
if (_limitState != LimitState.AtUpperLimit)
{
_limitState = LimitState.AtUpperLimit;
_impulse.z = 0.0f;
}
}
else
{
_limitState = LimitState.InactiveLimit;
_impulse.z = 0.0f;
}
}
else
{
_limitState = LimitState.InactiveLimit;
}
if (_enableMotor == false)
{
_motorImpulse = 0.0f;
}
if (step.WarmStarting)
{
//.........这里部分代码省略.........
开发者ID:imnotanderson，项目名称:Box2dUnity，代码行数:101，代码来源:PrismaticJoint.cs
示例8: initVelocityConstraints
public override void initVelocityConstraints(SolverData data)
{
m_indexA = m_bodyA.m_islandIndex;
m_indexB = m_bodyB.m_islandIndex;
m_localCenterA.set_Renamed(m_bodyA.m_sweep.localCenter);
m_localCenterB.set_Renamed(m_bodyB.m_sweep.localCenter);
m_invMassA = m_bodyA.m_invMass;
m_invMassB = m_bodyB.m_invMass;
m_invIA = m_bodyA.m_invI;
m_invIB = m_bodyB.m_invI;
Vec2 cA = data.positions[m_indexA].c;
float aA = data.positions[m_indexA].a;
Vec2 vA = data.velocities[m_indexA].v;
float wA = data.velocities[m_indexA].w;
Vec2 cB = data.positions[m_indexB].c;
float aB = data.positions[m_indexB].a;
Vec2 vB = data.velocities[m_indexB].v;
float wB = data.velocities[m_indexB].w;
Rot qA = pool.popRot();
Rot qB = pool.popRot();
Vec2 d = pool.popVec2();
Vec2 temp = pool.popVec2();
Vec2 rA = pool.popVec2();
Vec2 rB = pool.popVec2();
qA.set_Renamed(aA);
qB.set_Renamed(aB);
// Compute the effective masses.
Rot.mulToOutUnsafe(qA, d.set_Renamed(m_localAnchorA).subLocal(m_localCenterA), rA);
Rot.mulToOutUnsafe(qB, d.set_Renamed(m_localAnchorB).subLocal(m_localCenterB), rB);
d.set_Renamed(cB).subLocal(cA).addLocal(rB).subLocal(rA);
float mA = m_invMassA, mB = m_invMassB;
float iA = m_invIA, iB = m_invIB;
// Compute motor Jacobian and effective mass.
{
Rot.mulToOutUnsafe(qA, m_localXAxisA, m_axis);
temp.set_Renamed(d).addLocal(rA);
m_a1 = Vec2.cross(temp, m_axis);
m_a2 = Vec2.cross(rB, m_axis);
m_motorMass = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2;
if (m_motorMass > 0.0f)
{
m_motorMass = 1.0f / m_motorMass;
}
}
// Prismatic constraint.
{
Rot.mulToOutUnsafe(qA, m_localYAxisA, m_perp);
temp.set_Renamed(d).addLocal(rA);
m_s1 = Vec2.cross(temp, m_perp);
m_s2 = Vec2.cross(rB, m_perp);
float k11 = mA + mB + iA * m_s1 * m_s1 + iB * m_s2 * m_s2;
float k12 = iA * m_s1 + iB * m_s2;
float k13 = iA * m_s1 * m_a1 + iB * m_s2 * m_a2;
float k22 = iA + iB;
if (k22 == 0.0f)
{
// For bodies with fixed rotation.
k22 = 1.0f;
}
float k23 = iA * m_a1 + iB * m_a2;
float k33 = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2;
m_K.ex.set_Renamed(k11, k12, k13);
m_K.ey.set_Renamed(k12, k22, k23);
m_K.ez.set_Renamed(k13, k23, k33);
}
// Compute motor and limit terms.
if (m_enableLimit)
{
float jointTranslation = Vec2.dot(m_axis, d);
if (MathUtils.abs(m_upperTranslation - m_lowerTranslation) < 2.0f * Settings.linearSlop)
{
m_limitState = LimitState.EQUAL;
}
else if (jointTranslation <= m_lowerTranslation)
{
if (m_limitState != LimitState.AT_LOWER)
{
m_limitState = LimitState.AT_LOWER;
m_impulse.z = 0.0f;
}
}
else if (jointTranslation >= m_upperTranslation)
{
if (m_limitState != LimitState.AT_UPPER)
{
m_limitState = LimitState.AT_UPPER;
//.........这里部分代码省略.........
开发者ID:thdtjsdn，项目名称:box2dnet，代码行数:101，代码来源:PrismaticJoint.cs
示例9: InitVelocityConstraints
internal override void InitVelocityConstraints(TimeStep step)
{
Body b1 = _body1;
Body b2 = _body2;
if (_enableMotor || _enableLimit)
{
// You cannot create a rotation limit between bodies that
// both have fixed rotation.
Box2DXDebug.Assert(b1._invI > 0.0f || b2._invI > 0.0f);
}
// Compute the effective mass matrix.
Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ m1+r1y^2*i1+m2+r2y^2*i2, -r1y*i1*r1x-r2y*i2*r2x, -r1y*i1-r2y*i2]
// [ -r1y*i1*r1x-r2y*i2*r2x, m1+r1x^2*i1+m2+r2x^2*i2, r1x*i1+r2x*i2]
// [ -r1y*i1-r2y*i2, r1x*i1+r2x*i2, i1+i2]
float m1 = b1._invMass, m2 = b2._invMass;
float i1 = b1._invI, i2 = b2._invI;
_mass.Col1.X = m1 + m2 + r1.Y * r1.Y * i1 + r2.Y * r2.Y * i2;
_mass.Col2.X = -r1.Y * r1.X * i1 - r2.Y * r2.X * i2;
_mass.Col3.X = -r1.Y * i1 - r2.Y * i2;
_mass.Col1.Y = _mass.Col2.X;
_mass.Col2.Y = m1 + m2 + r1.X * r1.X * i1 + r2.X * r2.X * i2;
_mass.Col3.Y = r1.X * i1 + r2.X * i2;
_mass.Col1.Z = _mass.Col3.X;
_mass.Col2.Z = _mass.Col3.Y;
_mass.Col3.Z = i1 + i2;
_motorMass = 1.0f / (i1 + i2);
if (_enableMotor == false)
{
_motorImpulse = 0.0f;
}
if (_enableLimit)
{
float jointAngle = b2._sweep.A - b1._sweep.A - _referenceAngle;
if (Box2DXMath.Abs(_upperAngle - _lowerAngle) < 2.0f * Settings.AngularSlop)
{
_limitState = LimitState.EqualLimits;
}
else if (jointAngle <= _lowerAngle)
{
if (_limitState != LimitState.AtLowerLimit)
{
_impulse.Z = 0.0f;
}
_limitState = LimitState.AtLowerLimit;
}
else if (jointAngle >= _upperAngle)
{
if (_limitState != LimitState.AtUpperLimit)
{
_impulse.Z = 0.0f;
}
_limitState = LimitState.AtUpperLimit;
}
else
{
_limitState = LimitState.InactiveLimit;
_impulse.Z = 0.0f;
}
}
else
{
_limitState = LimitState.InactiveLimit;
}
if (step.WarmStarting)
{
// Scale impulses to support a variable time step.
_impulse *= step.DtRatio;
_motorImpulse *= step.DtRatio;
Vec2 P = new Vec2(_impulse.X, _impulse.Y);
b1._linearVelocity -= m1 * P;
b1._angularVelocity -= i1 * (Vec2.Cross(r1, P) + _motorImpulse + _impulse.Z);
b2._linearVelocity += m2 * P;
b2._angularVelocity += i2 * (Vec2.Cross(r2, P) + _motorImpulse + _impulse.Z);
}
else
{
_impulse.SetZero();
_motorImpulse = 0.0f;
}
}
开发者ID:danielpcox，项目名称:Crisis-at-Swiss-Station，代码行数:99，代码来源:RevoluteJoint.cs
示例10: InitVelocityConstraints
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body bA = BodyA;
Body bB = BodyB;
Transform xf1;
bA.GetTransform(out xf1);
Transform xf2;
bB.GetTransform(out xf2);
_rA = MathUtils.Multiply(ref xf1.R, LocalAnchorA - bA.LocalCenter);
_rB = MathUtils.Multiply(ref xf2.R, LocalAnchorB - bB.LocalCenter);
// Rope axis
_u = bB.Sweep.C + _rB - bA.Sweep.C - _rA;
_length = _u.Length;
float C = _length - MaxLength;
if (C > 0.0f)
{
_state = LimitState.AtUpper;
}
else
{
_state = LimitState.Inactive;
}
if (_length > Settings.LinearSlop)
{
_u *= 1.0f / _length;
}
else
{
_u = Vector2.Zero;
_mass = 0.0f;
_impulse = 0.0f;
return;
}
// Compute effective mass.
float crA = MathUtils.Cross(_rA, _u);
float crB = MathUtils.Cross(_rB, _u);
float invMass = bA.InvMass + bA.InvI * crA * crA + bB.InvMass + bB.InvI * crB * crB;
_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
if (Settings.EnableWarmstarting)
{
// Scale the impulse to support a variable time step.
_impulse *= step.dtRatio;
Vector2 P = _impulse * _u;
bA.LinearVelocity -= bA.InvMass * P;
bA.AngularVelocity -= bA.InvI * MathUtils.Cross(_rA, P);
bB.LinearVelocity += bB.InvMass * P;
bB.AngularVelocity += bB.InvI * MathUtils.Cross(_rB, P);
}
else
{
_impulse = 0.0f;
}
}
开发者ID:BraveSirAndrew，项目名称:farseerduality，代码行数:64，代码来源:RopeJoint.cs
示例11: InitVelocityConstraints
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body b1 = BodyA;
Body b2 = BodyB;
LocalCenterA = b1.LocalCenter;
LocalCenterB = b2.LocalCenter;
Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);
// Compute the effective masses.
Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - LocalCenterA);
Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - LocalCenterB);
Vector2 d = b2.Sweep.C + r2 - b1.Sweep.C - r1;
InvMassA = b1.InvMass;
InvIA = b1.InvI;
InvMassB = b2.InvMass;
InvIB = b2.InvI;
// Compute motor Jacobian and effective mass.
{
_axis = MathUtils.Multiply(ref xf1.R, _localXAxis1);
_a1 = MathUtils.Cross(d + r1, _axis);
_a2 = MathUtils.Cross(r2, _axis);
_motorMass = InvMassA + InvMassB + InvIA * _a1 * _a1 + InvIB * _a2 * _a2;
if (_motorMass > Settings.Epsilon)
{
_motorMass = 1.0f / _motorMass;
}
}
// Prismatic constraint.
{
_perp = MathUtils.Multiply(ref xf1.R, _localYAxis1);
_s1 = MathUtils.Cross(d + r1, _perp);
_s2 = MathUtils.Cross(r2, _perp);
float m1 = InvMassA, m2 = InvMassB;
float i1 = InvIA, i2 = InvIB;
float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float k12 = i1 * _s1 + i2 * _s2;
float k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
float k22 = i1 + i2;
float k23 = i1 * _a1 + i2 * _a2;
float k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;
_K.Col1 = new Vector3(k11, k12, k13);
_K.Col2 = new Vector3(k12, k22, k23);
_K.Col3 = new Vector3(k13, k23, k33);
}
// Compute motor and limit terms.
if (_enableLimit)
{
float jointTranslation = Vector2.Dot(_axis, d);
if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
_limitState = LimitState.Equal;
}
else if (jointTranslation <= _lowerTranslation)
{
if (_limitState != LimitState.AtLower)
{
_limitState = LimitState.AtLower;
_impulse.Z = 0.0f;
}
}
else if (jointTranslation >= _upperTranslation)
{
if (_limitState != LimitState.AtUpper)
{
_limitState = LimitState.AtUpper;
_impulse.Z = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
_impulse.Z = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
}
if (_enableMotor == false)
{
_motorImpulse = 0.0f;
}
if (Settings.EnableWarmstarting)
{
//.........这里部分代码省略.........
开发者ID:HaKDMoDz，项目名称:Zazumo，代码行数:101，代码来源:PrismaticJoint.cs
示例12: RevoluteJoint
internal RevoluteJoint(IWorldPool argWorld, RevoluteJointDef def)
: base(argWorld, def)
{
m_localAnchorA.set(def.localAnchorA);
m_localAnchorB.set(def.localAnchorB);
m_referenceAngle = def.referenceAngle;
m_motorImpulse = 0;
m_lowerAngle = def.lowerAngle;
m_upperAngle = def.upperAngle;
m_maxMotorTorque = def.maxMotorTorque;
m_motorSpeed = def.motorSpeed;
m_enableLimit = def.enableLimit;
m_enableMotor = def.enableMotor;
m_limitState = LimitState.INACTIVE;
}
开发者ID:Nomad1，项目名称:sharpbox2d，代码行数:17，代码来源:RevoluteJoint.cs
示例13: InitVelocityConstraints
internal override void InitVelocityConstraints(ref SolverData data)
{
_indexA = BodyA.IslandIndex;
_indexB = BodyB.IslandIndex;
_localCenterA = BodyA.Sweep.LocalCenter;
_localCenterB = BodyB.Sweep.LocalCenter;
_invMassA = BodyA.InvMass;
_invMassB = BodyB.InvMass;
_invIA = BodyA.InvI;
_invIB = BodyB.InvI;
Vector2 cA = data.positions[_indexA].c;
float aA = data.positions[_indexA].a;
Vector2 vA = data.velocities[_indexA].v;
float wA = data.velocities[_indexA].w;
Vector2 cB = data.positions[_indexB].c;
float aB = data.positions[_indexB].a;
Vector2 vB = data.velocities[_indexB].v;
float wB = data.velocities[_indexB].w;
Rot qA = new Rot(aA), qB = new Rot(aB);
_rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
_rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
_u = cB + _rB - cA - _rA;
_length = _u.Length();
float C = _length - MaxLength;
if (C > 0.0f)
{
_state = LimitState.AtUpper;
}
else
{
_state = LimitState.Inactive;
}
if (_length > Settings.LinearSlop)
{
_u *= 1.0f / _length;
}
else
{
_u = Vector2.Zero;
_mass = 0.0f;
_impulse = 0.0f;
return;
}
// Compute effective mass.
float crA = MathUtils.Cross(_rA, _u);
float crB = MathUtils.Cross(_rB, _u);
float invMass = _invMassA + _invIA * crA * crA + _invMassB + _invIB * crB * crB;
_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
if (Settings.EnableWarmstarting)
{
// Scale the impulse to support a variable time step.
_impulse *= data.step.dtRatio;
Vector2 P = _impulse * _u;
vA -= _invMassA * P;
wA -= _invIA * MathUtils.Cross(_rA, P);
vB += _invMassB * P;
wB += _invIB * MathUtils.Cross(_rB, P);
}
else
{
_impulse = 0.0f;
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
开发者ID:netonjm，项目名称:Rube.Net，代码行数:79，代码来源:RopeJoint.cs
示例14: InitVelocityConstraints
internal override void InitVelocityConstraints(TimeStep step)
{
Body body = this._body1;
Body body2 = this._body2;
Vec2 a = Box2DX.Common.Math.Mul(body.GetXForm().R, this._localAnchor1 - body.GetLocalCenter());
Vec2 a2 = Box2DX.Common.Math.Mul(body2.GetXForm().R, this._localAnchor2 - body2.GetLocalCenter());
float invMass = body._invMass;
float invMass2 = body2._invMass;
float invI = body._invI;
float invI2 = body2._invI;
this._mass.Col1.X = invMass + invMass2 + a.Y * a.Y * invI + a2.Y * a2.Y * invI2;
this._mass.Col2.X = -a.Y * a.X * invI - a2.Y * a2.X * invI2;
this._mass.Col3.X = -a.Y * invI - a2.Y * invI2;
this._mass.Col1.Y = this._mass.Col2.X;
this._mass.Col2.Y = invMass + invMass2 + a.X * a.X * invI + a2.X * a2.X * invI2;
this._mass.Col3.Y = a.X * invI + a2.X * invI2;
this._mass.Col1.Z = this._mass.Col3.X;
this._mass.Col2.Z = this._mass.Col3.Y;
this._mass.Col3.Z = invI + invI2;
this._motorMass = 1f / (invI + invI2);
if (!this._enableMotor)
{
this._motorImpulse = 0f;
}
if (this._enableLimit)
{
float num = body2._sweep.A - body._sweep.A - this._referenceAngle;
if (Box2DX.Common.Math.Abs(this._upperAngle - this._lowerAngle) < 2f * Settings.AngularSlop)
{
this._limitState = LimitState.EqualLimits;
}
else
{
if (num <= this._lowerAngle)
{
if (this._limitState != LimitState.AtLowerLimit)
{
this._impulse.Z = 0f;
}
this._limitState = LimitState.AtLowerLimit;
}
else
{
if (num >= this._upperAngle)
{
if (this._limitState != LimitState.AtUpperLimit)
{
this._impulse.Z = 0f;
}
this._limitState = LimitState.AtUpperLimit;
}
else
{
this._limitState = LimitState.InactiveLimit;
this._impulse.Z = 0f;
}
}
}
}
if (step.WarmStarting)
{
this._impulse *= step.DtRatio;
this._motorImpulse *= step.DtRatio;
Vec2 vec = new Vec2(this._impulse.X, this._impulse.Y);
Body expr_363 = body;
expr_363._linearVelocity -= invMass * vec;
body._angularVelocity -= invI * (Vec2.Cross(a, vec) + this._motorImpulse + this._impulse.Z);
Body expr_3A8 = body2;
expr_3A8._linearVelocity += invMass2 * vec;
body2._angularVelocity += invI2 * (Vec2.Cross(a2, vec) + this._motorImpulse + this._impulse.Z);
}
else
{
this._impulse.SetZero();
this._motorImpulse = 0f;
}
}
开发者ID:litdev1，项目名称:LitDev，代码行数:77，代码来源:RevoluteJoint.cs
示例15: PrismaticJoint
public PrismaticJoint(PrismaticJointDef def)
: base(def)
{
_localAnchor1 = def.LocalAnchor1;
_localAnchor2 = def.LocalAnchor2;
_localXAxis1 = def.LocalAxis1;
_localYAxis1 = _localXAxis1.CrossScalarPreMultiply(1.0f);
_refAngle = def.ReferenceAngle;
_impulse = Vector3.zero;
_motorMass = 0.0f;
_motorImpulse = 0.0f;
_lowerTranslation = def.LowerTranslation;
_upperTranslation = def.UpperTranslation;
_maxMotorForce = Settings.FORCE_INV_SCALE(def.MaxMotorForce);
_motorSpeed = def.MotorSpeed;
_enableLimit = def.EnableLimit;
_enableMotor = def.EnableMotor;
_limitState = LimitState.InactiveLimit;
_axis = Vector2.zero;
_perp= Vector2.zero;
}
开发者ID:imnotanderson，项目名称:Box2dUnity，代码行数:24，代码来源:PrismaticJoint.cs
示例16: FixedRevoluteJoint
/// <summary>
/// Initialize the bodies, anchors, and reference angle using the world
/// anchor.
/// This requires defining an
/// anchor point where the bodies are joined. The definition
/// uses local anchor points so that the initial configuration
/// can violate the constraint slightly. You also need to
/// specify the initial relative angle for joint limits. This
/// helps when saving and loading a game.
/// The local anchor points are measured from the body's origin
/// rather than the center of mass because:
/// 1. you might not know where the center of mass will be.
/// 2. if you add/remove shapes from a body and recompute the mass,
/// the joints will be broken.
/// </summary>
/// <param name="body">The body.</param>
/// <param name="bodyanchor">The body anchor.</param>
/// <param name="worldanchor">The world anchor.</param>
public FixedRevoluteJoint(Body body, Vector2 bodyanchor, Vector2 worldanchor)
: base(body)
{
JointType = JointType.FixedRevolute;
// Changed to local coordinates.
LocalAnchorA = bodyanchor;
LocalAnchorB = worldanchor;
ReferenceAngle = -BodyA.Rotation;
_impulse = Vector3.Zero;
_limitState = LimitState.Inactive;
}
开发者ID:HaKDMoDz，项目名称:Lunar-Development-Kit，代码行数:33，代码来源:FixedRevoluteJoint.cs
示例17: InitVelocityConstraints
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body b1 = _bodyA;
Body b2 = _bodyB;
XForm xf1, xf2;
b1.GetXForm(out xf1);
b2.GetXForm(out xf2);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());
Vector2 p1 = b1._sweep.c + r1;
Vector2 p2 = b2._sweep.c + r2;
Vector2 s1 = _groundAnchor1;
Vector2 s2 = _groundAnchor2;
// Get the pulley axes.
_u1 = p1 - s1;
_u2 = p2 - s2;
float length1 = _u1.Length();
float length2 = _u2.Length();
if (length1 > Settings.b2_linearSlop)
{
_u1 *= 1.0f / length1;
}
else
{
_u1 = Vector2.Zero;
}
if (length2 > Settings.b2_linearSlop)
{
_u2 *= 1.0f / length2;
}
else
{
_u2 = Vector2.Zero;
}
float C = _ant - length1 - _ratio * length2;
if (C > 0.0f)
{
_state = LimitState.Inactive;
_impulse = 0.0f;
}
else
{
_state = LimitState.AtUpper;
}
if (length1 < _maxLength1)
{
_limitState1 = LimitState.Inactive;
_limitImpulse1 = 0.0f;
}
else
{
_limitState1 = LimitState.AtUpper;
}
if (length2 < _maxLength2)
{
_limitState2 = LimitState.Inactive;
_limitImpulse2 = 0.0f;
}
else
{
_limitState2 = LimitState.AtUpper;
}
// Compute effective mass.
float cr1u1 = MathUtils.Cross(r1, _u1);
float cr2u2 = MathUtils.Cross(r2, _u2);
_limitMass1 = b1._invMass + b1._invI * cr1u1 * cr1u1;
_limitMass2 = b2._invMass + b2._invI * cr2u2 * cr2u2;
_pulleyMass = _limitMass1 + _ratio * _ratio * _limitMass2;
Debug.Assert(_limitMass1 > Settings.b2_FLT_EPSILON);
Debug.Assert(_limitMass2 > Settings.b2_FLT_EPSILON);
Debug.Assert(_pulleyMass > Settings.b2_FLT_EPSILON);
_limitMass1 = 1.0f / _limitMass1;
_limitMass2 = 1.0f / _limitMass2;
_pulleyMass = 1.0f / _pulleyMass;
if (step.warmStarting)
{
// Scale impulses to support variable time steps.
_impulse *= step.dtRatio;
_limitImpulse1 *= step.dtRatio;
_limitImpulse2 *= step.dtRatio;
// Warm starting.
Vector2 P1 = -(_impulse + _limitImpulse1) * _u1;
Vector2 P2 = (-_ratio * _impulse - _limitImpulse2) * _u2;
b1._linearVelocity += b1._invMass * P1;
b1._angularVelocity += b1._invI * MathUtils.Cross(r1, P1);
//.........这里部分代码省略.........
开发者ID:Nukepayload2，项目名称:Box2D，代码行数:101，代码来源:PullyJoint.cs
示例18: RevoluteJoint
public RevoluteJoint(RevoluteJointDef def)
: base(def)
{
_localAnchor1 = def.LocalAnchor1;
_localAnchor2 = def.LocalAnchor2;
_referenceAngle = def.ReferenceAngle;
_impulse = new Vec3();
_motorImpulse = 0.0f;
_lowerAngle = def.LowerAngle;
_upperAngle = def.UpperAngle;
_maxMotorTorque = def.MaxMotorTorque;
_motorSpeed = def.MotorSpeed;
_enableLimit = def.EnableLimit;
_enableMotor = def.EnableMotor;
_limitState = LimitState.InactiveLimit;
}
开发者ID:danielpcox，项目名称:Crisis-at-Swiss-Station，代码行数:18，代码来源:RevoluteJoint.cs
示例19: initVelocityConstraints
public override void initVelocityConstraints(SolverData data)
{
m_indexA = m_bodyA.m_islandIndex;
m_indexB = m_bodyB.m_islandIndex;
m_localCenterA.set(m_bodyA.m_sweep.localCenter);
m_localCenterB.set(m_bodyB.m_sweep.localCenter);
m_invMassA = m_bodyA.m_invMass;
m_invMassB = m_bodyB.m_invMass;
m_invIA = m_bodyA.
|
客服电话
APP下载
官方微信
请发表评论