// constructor // anchor, axis1 and axis2 are in world coordinate system // axis1 must be orthogonal to axis2 public UniversalConstraint(RigidBody rbA, RigidBody rbB, ref Vector3 anchor, ref Vector3 axis1, ref Vector3 axis2) : base(rbA, rbB, ref BulletGlobals.IdentityMatrix, ref BulletGlobals.IdentityMatrix, true) { m_anchor = anchor; m_axis1 = axis1; m_axis2 = axis2; // build frame basis // 6DOF constraint uses Euler angles and to define limits // it is assumed that rotational order is : // Z - first, allowed limits are (-PI,PI); // new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number // used to prevent constraint from instability on poles; // new position of X, allowed limits are (-PI,PI); // So to simulate ODE Universal joint we should use parent axis as Z, child axis as Y and limit all other DOFs // Build the frame in world coordinate system first Vector3 zAxis = Vector3.Normalize(axis1); Vector3 yAxis = Vector3.Normalize(axis2); Vector3 xAxis = Vector3.Cross(yAxis,zAxis); // we want right coordinate system Matrix frameInW = Matrix.Identity; MathUtil.SetBasis(ref frameInW,ref xAxis,ref yAxis,ref zAxis); frameInW.Translation = anchor; // now get constraint frame in local coordinate systems //m_frameInA = MathUtil.inverseTimes(rbA.getCenterOfMassTransform(),frameInW); //m_frameInB = MathUtil.inverseTimes(rbB.getCenterOfMassTransform(),frameInW); m_frameInA = MathUtil.BulletMatrixMultiply(Matrix.Invert(rbA.GetCenterOfMassTransform()), frameInW); m_frameInB = MathUtil.BulletMatrixMultiply(Matrix.Invert(rbB.GetCenterOfMassTransform()), frameInW); // sei limits SetLinearLowerLimit(Vector3.Zero); SetLinearUpperLimit(Vector3.Zero); SetAngularLowerLimit(new Vector3(0.0f, -MathUtil.SIMD_HALF_PI + UNIV_EPS, -MathUtil.SIMD_PI + UNIV_EPS)); SetAngularUpperLimit(new Vector3(0.0f, MathUtil.SIMD_HALF_PI - UNIV_EPS, MathUtil.SIMD_PI - UNIV_EPS)); }
public RaycastVehicle(VehicleTuning tuning, RigidBody chassis, IVehicleRaycaster raycaster) { m_vehicleRaycaster = raycaster; m_pitchControl = 0f; m_chassisBody = chassis; m_indexRightAxis = 0; m_indexUpAxis = 1; m_indexForwardAxis = 2; DefaultInit(ref tuning); }
public Generic6DofSpringConstraint(RigidBody rbA, RigidBody rbB, ref Matrix frameInA, ref Matrix frameInB ,bool useLinearReferenceFrameA) : base(rbA,rbB,ref frameInA,ref frameInB,useLinearReferenceFrameA) { for (int i = 0; i < s_degreesOfFreedom; ++i) { m_springEnabled[i] = false; m_equilibriumPoint[i] = 0.0f; m_springStiffness[i] = 0.0f; m_springDamping[i] = 1.0f; } }
public WheelContactPoint(RigidBody body0,RigidBody body1,ref Vector3 frictionPosWorld,ref Vector3 frictionDirectionWorld, float maxImpulse) { m_body0 = body0; m_body1 = body1; m_frictionPositionWorld = frictionPosWorld; m_frictionDirectionWorld = frictionDirectionWorld; m_maxImpulse = maxImpulse; float denom0 = body0.ComputeImpulseDenominator(ref frictionPosWorld,ref frictionDirectionWorld); float denom1 = body1.ComputeImpulseDenominator(ref frictionPosWorld,ref frictionDirectionWorld); float relaxation = 1f; m_jacDiagABInv = relaxation/(denom0+denom1); }
///bilateral constraint between two dynamic objects ///positive distance = separation, negative distance = penetration public static void ResolveSingleBilateral(RigidBody body1, ref Vector3 pos1, RigidBody body2, ref Vector3 pos2, float distance, ref Vector3 normal, ref float impulse, float timeStep) { float normalLenSqr = normal.LengthSquared(); Debug.Assert(System.Math.Abs(normalLenSqr) < 1.1f); if (normalLenSqr > 1.1f) { impulse = 0f; return; } Vector3 rel_pos1 = pos1 - body1.GetCenterOfMassPosition(); Vector3 rel_pos2 = pos2 - body2.GetCenterOfMassPosition(); //this jacobian entry could be re-used for all iterations Vector3 vel1 = body1.GetVelocityInLocalPoint(ref rel_pos1); Vector3 vel2 = body2.GetVelocityInLocalPoint(ref rel_pos2); Vector3 vel = vel1 - vel2; Matrix m1 = MathUtil.TransposeBasis(body1.GetCenterOfMassTransform()); Matrix m2 = MathUtil.TransposeBasis(body2.GetCenterOfMassTransform()); JacobianEntry jac = new JacobianEntry(m1,m2,rel_pos1,rel_pos2,normal, body1.GetInvInertiaDiagLocal(),body1.GetInvMass(), body2.GetInvInertiaDiagLocal(),body2.GetInvMass()); float jacDiagAB = jac.GetDiagonal(); float jacDiagABInv = 1f / jacDiagAB; float rel_vel = jac.GetRelativeVelocity( body1.GetLinearVelocity(),Vector3.TransformNormal(body1.GetAngularVelocity(),m1), body2.GetLinearVelocity(),Vector3.TransformNormal(body2.GetAngularVelocity(),m2)); float a = jacDiagABInv; rel_vel = Vector3.Dot(normal,vel); //todo: move this into proper structure float contactDamping = 0.2f; if(ONLY_USE_LINEAR_MASS) { float massTerm = 1f / (body1.GetInvMass() + body2.GetInvMass()); impulse = - contactDamping * rel_vel * massTerm; } else { float velocityImpulse = -contactDamping * rel_vel * jacDiagABInv; impulse = velocityImpulse; } }
public TypedConstraint(TypedConstraintType type, RigidBody rbA) : base((int)type) { m_userConstraintType = -1; m_userConstraintId = -1; m_constraintType = type; m_rbA = rbA; m_rbB = GetFixedBody(); m_appliedImpulse = 0f; m_dbgDrawSize = DEFAULT_DEBUGDRAW_SIZE; { s_fixed.SetMassProps(0f,Vector3.Zero); } }
public Generic6DofConstraint(RigidBody rbA, RigidBody rbB, ref Matrix frameInA, ref Matrix frameInB ,bool useLinearReferenceFrameA) : base(TypedConstraintType.D6_CONSTRAINT_TYPE,rbA,rbB) { m_frameInA = frameInA; m_frameInB = frameInB; m_useLinearReferenceFrameA = useLinearReferenceFrameA; m_useOffsetForConstraintFrame = D6_USE_FRAME_OFFSET; m_flags = 0; m_linearLimits = new TranslationalLimitMotor(); m_angularLimits[0] = new RotationalLimitMotor(); m_angularLimits[1] = new RotationalLimitMotor(); m_angularLimits[2] = new RotationalLimitMotor(); CalculateTransforms(); }
public HingeConstraint(RigidBody rbA, RigidBody rbB, ref Vector3 pivotInA, ref Vector3 pivotInB, ref Vector3 axisInA, ref Vector3 axisInB, bool useReferenceFrameA) : base(TypedConstraintType.HINGE_CONSTRAINT_TYPE,rbA,rbB) { m_angularOnly = false; m_enableAngularMotor = false; m_useOffsetForConstraintFrame = HINGE_USE_FRAME_OFFSET; m_useReferenceFrameA = useReferenceFrameA; m_rbAFrame.Translation = pivotInA; m_flags = 0; // since no frame is given, assume this to be zero angle and just pick rb transform axis Vector3 rbAxisA1 = MathUtil.MatrixColumn(rbA.GetCenterOfMassTransform(),0); Vector3 rbAxisA2 = Vector3.Zero; float projection = Vector3.Dot(axisInA,rbAxisA1); if (projection >= 1.0f - MathUtil.SIMD_EPSILON) { rbAxisA1 = -MathUtil.MatrixColumn(rbA.GetCenterOfMassTransform(),2); rbAxisA2 = MathUtil.MatrixColumn(rbA.GetCenterOfMassTransform(),1); } else if (projection <= -1.0f + MathUtil.SIMD_EPSILON) { rbAxisA1 = MathUtil.MatrixColumn(rbA.GetCenterOfMassTransform(),2); rbAxisA2 = MathUtil.MatrixColumn(rbA.GetCenterOfMassTransform(),1); } else { rbAxisA2 = Vector3.Cross(axisInA,rbAxisA1); rbAxisA1 = Vector3.Cross(rbAxisA2,axisInA); } MathUtil.SetBasis(ref m_rbAFrame,ref rbAxisA1,ref rbAxisA2,ref axisInA); Quaternion rotationArc = MathUtil.ShortestArcQuat(ref axisInA, ref axisInB); Vector3 rbAxisB1 = MathUtil.QuatRotate(ref rotationArc, ref rbAxisA1); Vector3 rbAxisB2 = Vector3.Cross(axisInB,rbAxisB1); m_rbBFrame.Translation = pivotInB; MathUtil.SetBasis(ref m_rbBFrame,ref rbAxisB1,ref rbAxisB2,ref axisInB); //start with free m_lowerLimit = 1f; m_upperLimit = -1f; m_biasFactor = 0.3f; m_relaxationFactor = 1.0f; m_limitSoftness = 0.9f; m_solveLimit = false; m_referenceSign = m_useReferenceFrameA ? -1f : 1f; }
protected Generic6DofConstraint(RigidBody rbB, ref Matrix frameInB, bool useLinearReferenceFrameB) : base(TypedConstraintType.D6_CONSTRAINT_TYPE,GetFixedBody(),rbB) { m_frameInB = frameInB; m_useLinearReferenceFrameA = useLinearReferenceFrameB; m_useOffsetForConstraintFrame = D6_USE_FRAME_OFFSET; m_flags = 0; m_linearLimits = new TranslationalLimitMotor(); m_angularLimits[0] = new RotationalLimitMotor(); m_angularLimits[1] = new RotationalLimitMotor(); m_angularLimits[2] = new RotationalLimitMotor(); ///not providing rigidbody A means implicitly using worldspace for body A m_frameInA = MathUtil.BulletMatrixMultiply(rbB.GetCenterOfMassTransform(),m_frameInB); CalculateTransforms(); }
// constructor // anchor, axis1 and axis2 are in world coordinate system // axis1 must be orthogonal to axis2 public Hinge2Constraint(RigidBody rbA, RigidBody rbB, ref Vector3 anchor, ref Vector3 axis1, ref Vector3 axis2) : base(rbA,rbB,Matrix.Identity,Matrix.Identity,true) { m_anchor = anchor; m_axis1 = axis1; m_axis2 = axis2; // build frame basis // 6DOF constraint uses Euler angles and to define limits // it is assumed that rotational order is : // Z - first, allowed limits are (-PI,PI); // new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number // used to prevent constraint from instability on poles; // new position of X, allowed limits are (-PI,PI); // So to simulate ODE Universal joint we should use parent axis as Z, child axis as Y and limit all other DOFs // Build the frame in world coordinate system first Vector3 zAxis = Vector3.Normalize(axis1); Vector3 xAxis = Vector3.Normalize(axis2); Vector3 yAxis = Vector3.Cross(zAxis,xAxis); // we want right coordinate system Matrix frameInW = Matrix.Identity; MathUtil.SetBasis(ref frameInW, ref xAxis, ref yAxis, ref zAxis); frameInW.Translation = anchor; // now get constraint frame in local coordinate systems m_frameInA = MathUtil.InverseTimes(rbA.GetCenterOfMassTransform(),frameInW); m_frameInB = MathUtil.InverseTimes(rbB.GetCenterOfMassTransform(), frameInW); // sei limits SetLinearLowerLimit(new Vector3(0.0f, 0.0f, -1.0f)); SetLinearUpperLimit(new Vector3(0.0f, 0.0f, 1.0f)); // like front wheels of a car SetAngularLowerLimit(new Vector3(1.0f, 0.0f, -MathUtil.SIMD_HALF_PI * 0.5f)); SetAngularUpperLimit(new Vector3(-1.0f, 0.0f, MathUtil.SIMD_HALF_PI * 0.5f)); // enable suspension EnableSpring(2, true); SetStiffness(2, MathUtil.SIMD_PI * MathUtil.SIMD_PI * 4.0f); // period 1 sec for 1 kilogramm weel :-) SetDamping(2, 0.01f); SetEquilibriumPoint(); }
public float SolveLinearAxis( float timeStep, float jacDiagABInv, RigidBody body1, ref Vector3 pointInA, RigidBody body2, ref Vector3 pointInB, int limit_index, ref Vector3 axis_normal_on_a, ref Vector3 anchorPos) { ///find relative velocity // Vector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition(); // Vector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition(); Vector3 rel_pos1 = anchorPos - body1.GetCenterOfMassPosition(); Vector3 rel_pos2 = anchorPos - body2.GetCenterOfMassPosition(); Vector3 vel1 = Vector3.Zero; body1.InternalGetVelocityInLocalPointObsolete(ref rel_pos1,ref vel1); Vector3 vel2 = Vector3.Zero;; body2.InternalGetVelocityInLocalPointObsolete(ref rel_pos2,ref vel2); Vector3 vel = vel1 - vel2; float rel_vel = Vector3.Dot(axis_normal_on_a,vel); /// apply displacement correction //positional error (zeroth order error) float depth = -Vector3.Dot((pointInA - pointInB),axis_normal_on_a); float lo = float.MinValue; float hi = float.MaxValue; float minLimit = MathUtil.VectorComponent(ref m_lowerLimit,limit_index); float maxLimit = MathUtil.VectorComponent(ref m_upperLimit,limit_index); //handle the limits if (minLimit < maxLimit) { { if (depth > maxLimit) { depth -= maxLimit; lo = 0f; } else { if (depth < minLimit) { depth -= minLimit; hi = 0f; } else { return 0.0f; } } } } float normalImpulse= m_limitSoftness*(m_restitution*depth/timeStep - m_damping*rel_vel) * jacDiagABInv; float oldNormalImpulse = MathUtil.VectorComponent(ref m_accumulatedImpulse,limit_index); float sum = oldNormalImpulse + normalImpulse; MathUtil.VectorComponent(ref m_accumulatedImpulse,limit_index,(sum > hi ? 0f: sum < lo ? 0f : sum)); normalImpulse = MathUtil.VectorComponent(ref m_accumulatedImpulse,limit_index) - oldNormalImpulse; Vector3 impulse_vector = axis_normal_on_a * normalImpulse; //body1.applyImpulse( impulse_vector, rel_pos1); //body2.applyImpulse(-impulse_vector, rel_pos2); Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos1,axis_normal_on_a); Vector3 ftorqueAxis2 = Vector3.Cross(rel_pos2,axis_normal_on_a); body1.InternalApplyImpulse(axis_normal_on_a*body1.GetInvMass(), Vector3.TransformNormal(ftorqueAxis1,body1.GetInvInertiaTensorWorld()),normalImpulse); body2.InternalApplyImpulse(axis_normal_on_a * body2.GetInvMass(), Vector3.TransformNormal(ftorqueAxis2,body2.GetInvInertiaTensorWorld()), -normalImpulse); return normalImpulse; }
public HingeConstraint(RigidBody rbA, RigidBody rbB, ref Vector3 pivotInA, ref Vector3 pivotInB, ref Vector3 axisInA, ref Vector3 axisInB) : this(rbA, rbB, ref pivotInA,ref pivotInB, ref axisInA, ref axisInB,false) { }
private static void Main(string[] args) { var collisionConfig = new DefaultCollisionConfiguration(); var dispatcher = new CollisionDispatcher(collisionConfig); var pairCache = new DbvtBroadphase(); var solver = new SequentialImpulseConstraintSolver(); var dynamicsWorld = new DiscreteDynamicsWorld(dispatcher, pairCache, solver, collisionConfig); dynamicsWorld.Gravity = new Vector3(0, -10, 0); var groundShape = new BoxShape(new Vector3(50, 50, 50)); var groundTransform = Matrix.CreateTranslation(0, -56, 0); { float mass = 0; bool isDynamic = mass != 0; Vector3 localInertia = Vector3.Zero; if (isDynamic) localInertia = groundShape.CalculateLocalInertia(mass); var motionState = new DefaultMotionState(groundTransform); var rbinfo = new RigidBodyConstructionInfo(mass, motionState, groundShape, localInertia); var body = new RigidBody(rbinfo); dynamicsWorld.AddRigidBody(body); } { var collisionShape = new SphereShape(1); var transform = Matrix.Identity; float mass = 1; bool isDynamic = mass != 0; Vector3 localInertia = Vector3.Zero; if (isDynamic) localInertia = collisionShape.CalculateLocalInertia(mass); transform.Translation = new Vector3(2, 10, 0); var motionState = new DefaultMotionState(transform); var rbinfo = new RigidBodyConstructionInfo(mass, motionState, collisionShape, localInertia); var body = new RigidBody(rbinfo); dynamicsWorld.AddRigidBody(body); } Console.WriteLine("Starting simulation"); for (int i = 0; i < 100; i++) { dynamicsWorld.StepSimulation(1f / 60f, 10); for (int j = dynamicsWorld.GetNumCollisionObjects() - 1; j >= 0; j--) { var obj = dynamicsWorld.GetCollisionObjectArray()[j]; var body = (RigidBody)obj; if (body != null && body.GetMotionState() != null) { Matrix transform = Matrix.Identity; body.GetMotionState().GetWorldTransform(ref transform); Console.WriteLine("Object@{0} Position={1}", body.GetHashCode(), transform.Translation); } } } Console.Read(); }
public ContactConstraint(PersistentManifold contactManifold, RigidBody rbA, RigidBody rbB) : base(TypedConstraintType.CONTACT_CONSTRAINT_TYPE,rbA,rbB) { m_contactManifold = contactManifold; }
public Point2PointConstraint(RigidBody rbA, RigidBody rbB, ref Vector3 pivotInA, ref Vector3 pivotInB) : base(TypedConstraintType.POINT2POINT_CONSTRAINT_TYPE,rbA,rbB) { m_pivotInA = pivotInA; m_pivotInB = pivotInB; }
//! apply the correction impulses for two bodies public float SolveAngularLimits(float timeStep,ref Vector3 axis, float jacDiagABInv,RigidBody body0, RigidBody body1) { if (NeedApplyTorques() == false) { return 0.0f; } float target_velocity = m_targetVelocity; float maxMotorForce = m_maxMotorForce; //current error correction if (m_currentLimit!=0) { target_velocity = -m_stopERP*m_currentLimitError/(timeStep); maxMotorForce = m_maxLimitForce; } maxMotorForce *= timeStep; // current velocity difference Vector3 angVelA = Vector3.Zero; body0.InternalGetAngularVelocity(ref angVelA); Vector3 angVelB = Vector3.Zero; body1.InternalGetAngularVelocity(ref angVelB); Vector3 vel_diff = angVelA-angVelB; float rel_vel = Vector3.Dot(axis,vel_diff); // correction velocity float motor_relvel = m_limitSoftness*(target_velocity - m_damping*rel_vel); if ( motor_relvel < MathUtil.SIMD_EPSILON && motor_relvel > -MathUtil.SIMD_EPSILON ) { return 0.0f;//no need for applying force } // correction impulse float unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv; // clip correction impulse float clippedMotorImpulse; ///@todo: should clip against accumulated impulse if (unclippedMotorImpulse>0.0f) { clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse; } else { clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse; } // sort with accumulated impulses float lo = float.MinValue; float hi = float.MaxValue; float oldaccumImpulse = m_accumulatedImpulse; float sum = oldaccumImpulse + clippedMotorImpulse; m_accumulatedImpulse = sum > hi ? 0f : sum < lo ? 0f : sum; clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse; Vector3 motorImp = clippedMotorImpulse * axis; //body0.applyTorqueImpulse(motorImp); //body1.applyTorqueImpulse(-motorImp); body0.InternalApplyImpulse(Vector3.Zero, Vector3.TransformNormal(axis,body0.GetInvInertiaTensorWorld()),clippedMotorImpulse); body1.InternalApplyImpulse(Vector3.Zero, Vector3.TransformNormal(axis,body1.GetInvInertiaTensorWorld()),-clippedMotorImpulse); return clippedMotorImpulse; }
public abstract void RemoveRigidBody(RigidBody body);
//public abstract void addRigidBody(RigidBody body,short mask1,short mask2); public abstract void AddRigidBody(RigidBody body);
public override void RemoveRigidBody(RigidBody body) { m_nonStaticRigidBodies.Remove(body); base.RemoveCollisionObject(body); }
public virtual void AddRigidBody(RigidBody body, CollisionFilterGroups group, CollisionFilterGroups mask) { if (!body.IsStaticOrKinematicObject() && 0 == (body.GetFlags() & RigidBodyFlags.BT_DISABLE_WORLD_GRAVITY)) body.Gravity = m_gravity; if (body.GetCollisionShape() != null) { if (!body.IsStaticObject()) { m_nonStaticRigidBodies.Add(body); } else { body.SetActivationState(ActivationState.ISLAND_SLEEPING); } AddCollisionObject(body, group, mask); } }
public override void AddRigidBody(RigidBody body) { if (!body.IsStaticOrKinematicObject() && 0 == (body.GetFlags() & RigidBodyFlags.BT_DISABLE_WORLD_GRAVITY)) body.Gravity = m_gravity; if (body.GetCollisionShape() != null) { if (!body.IsStaticObject()) { m_nonStaticRigidBodies.Add(body); } else { body.SetActivationState(ActivationState.ISLAND_SLEEPING); } bool isDynamic = !(body.IsStaticObject() || body.IsKinematicObject()); CollisionFilterGroups collisionFilterGroup = isDynamic ? CollisionFilterGroups.DefaultFilter : CollisionFilterGroups.StaticFilter; CollisionFilterGroups collisionFilterMask = isDynamic ? CollisionFilterGroups.AllFilter : (CollisionFilterGroups.AllFilter ^ CollisionFilterGroups.StaticFilter); AddCollisionObject(body, collisionFilterGroup, collisionFilterMask); } }
///this can be useful to synchronize a single rigid body . graphics object public void SynchronizeSingleMotionState(RigidBody body) { Debug.Assert(body != null); if (body.GetMotionState() != null && !body.IsStaticOrKinematicObject()) { //we need to call the update at least once, even for sleeping objects //otherwise the 'graphics' transform never updates properly ///@todo: add 'dirty' flag //if (body.getActivationState() != ISLAND_SLEEPING) { Matrix interpolatedTransform = Matrix.Identity; TransformUtil.IntegrateTransform(body.GetInterpolationWorldTransform(), body.SetInterpolationLinearVelocity(), body.GetInterpolationAngularVelocity(), m_localTime * body.GetHitFraction(), ref interpolatedTransform); body.GetMotionState().SetWorldTransform(ref interpolatedTransform); } } }
public void UpdateWheel(RigidBody chassis, ref WheelRaycastInfo raycastInfo) { if (m_raycastInfo.m_isInContact) { float project= Vector3.Dot(m_raycastInfo.m_contactNormalWS,m_raycastInfo.m_wheelDirectionWS ); Vector3 chassis_velocity_at_contactPoint; Vector3 relpos = m_raycastInfo.m_contactPointWS - chassis.GetCenterOfMassPosition(); chassis_velocity_at_contactPoint = chassis.GetVelocityInLocalPoint( ref relpos ); float projVel = Vector3.Dot(m_raycastInfo.m_contactNormalWS,chassis_velocity_at_contactPoint ); if ( project >= -0.1f) { m_suspensionRelativeVelocity = 0f; m_clippedInvContactDotSuspension = 1.0f / 0.1f; } else { float inv = -1f / project; m_suspensionRelativeVelocity = projVel * inv; m_clippedInvContactDotSuspension = inv; } } else // Not in contact : position wheel in a nice (rest length) position { m_raycastInfo.m_suspensionLength = this.GetSuspensionRestLength(); m_suspensionRelativeVelocity = 0f; m_raycastInfo.m_contactNormalWS = -m_raycastInfo.m_wheelDirectionWS; m_clippedInvContactDotSuspension = 1f; } }
public HingeConstraint(RigidBody rbA, ref Vector3 pivotInA, ref Vector3 axisInA, bool useReferenceFrameA) : base(TypedConstraintType.HINGE_CONSTRAINT_TYPE,rbA) { m_angularOnly = false; m_enableAngularMotor = false; m_useReferenceFrameA = useReferenceFrameA; m_useOffsetForConstraintFrame = HINGE_USE_FRAME_OFFSET; m_flags = 0; // since no frame is given, assume this to be zero angle and just pick rb transform axis // fixed axis in worldspace Vector3 rbAxisA1 = Vector3.Zero, rbAxisA2 = Vector3.Zero; TransformUtil.PlaneSpace1(ref axisInA, ref rbAxisA1, ref rbAxisA2); m_rbAFrame.Translation = pivotInA; MathUtil.SetBasis(ref m_rbAFrame,ref rbAxisA1,ref rbAxisA2,ref axisInA); Vector3 axisInB = Vector3.TransformNormal(axisInA,rbA.GetCenterOfMassTransform()); Quaternion rotationArc = MathUtil.ShortestArcQuat(ref axisInA, ref axisInB); Vector3 rbAxisB1 = MathUtil.QuatRotate(ref rotationArc, ref rbAxisA1); Vector3 rbAxisB2 = Vector3.Cross(axisInB,rbAxisB1); m_rbBFrame.Translation = Vector3.Transform(pivotInA,rbA.GetCenterOfMassTransform()); MathUtil.SetBasis(ref m_rbBFrame, ref rbAxisB1, ref rbAxisB2, ref axisInB); //start with free m_lowerLimit = 1f; m_upperLimit = -1f; m_biasFactor = 0.3f; m_relaxationFactor = 1.0f; m_limitSoftness = 0.9f; m_solveLimit = false; m_referenceSign = m_useReferenceFrameA ? -1.0f : 1.0f; }
public Point2PointConstraint(RigidBody rbA,ref Vector3 pivotInA) : base(TypedConstraintType.POINT2POINT_CONSTRAINT_TYPE,rbA) { m_pivotInA = pivotInA; m_pivotInB = Vector3.Transform(pivotInA, rbA.GetCenterOfMassTransform()); }
public HingeConstraint(RigidBody rbA, ref Matrix rbAFrame) : this(rbA, ref rbAFrame, false) { }
public override void RemoveRigidBody(RigidBody body) { base.RemoveCollisionObject(body); }
public HingeConstraint(RigidBody rbA, ref Matrix rbAFrame, bool useReferenceFrameA) : base(TypedConstraintType.HINGE_CONSTRAINT_TYPE,rbA) { m_rbAFrame = rbAFrame; m_rbBFrame = rbAFrame; m_angularOnly = false; m_enableAngularMotor = false; m_useOffsetForConstraintFrame = HINGE_USE_FRAME_OFFSET; m_useReferenceFrameA = useReferenceFrameA; m_flags = 0; m_rbBFrame.Translation = Vector3.Transform(m_rbAFrame.Translation,m_rbA.GetCenterOfMassTransform()); //start with free m_lowerLimit = 1f; m_upperLimit = -1f; m_biasFactor = 0.3f; m_relaxationFactor = 1.0f; m_limitSoftness = 0.9f; m_solveLimit = false; m_referenceSign = m_useReferenceFrameA ? -1.0f : 1.0f; }
public ConeTwistConstraint(RigidBody rbA, ref Matrix rbAFrame) : base(TypedConstraintType.CONETWIST_CONSTRAINT_TYPE, rbA) { m_rbAFrame = rbAFrame; m_rbBFrame = rbAFrame; m_angularOnly = false; Init(); }
public Generic6DofSpringConstraint(RigidBody rbA, RigidBody rbB, Matrix frameInA, Matrix frameInB, bool useLinearReferenceFrameA) : this(rbA, rbB, ref frameInA, ref frameInB, useLinearReferenceFrameA) { }