C# btVector3.dot方法代码示例

		btStaticPlaneShape( ref btVector3 planeOrigin, ref btVector3 planeNormal ) : base()
		{
			planeNormal.normalized( out m_planeNormal );
			m_planeConstant = planeOrigin.dot( ref planeNormal );
			m_localScaling = btVector3.Zero;
			m_shapeType = BroadphaseNativeTypes.STATIC_PLANE_PROXYTYPE;
			//	Debug.Assert( btFuzzyZero(m_planeNormal.length() - btScalar.BT_ONE) );
		}

		static bool notExist( ref btVector3 planeEquation, btList<btVector3> planeEquations )
		{
			int numbrushes = planeEquations.Count;
			for( int i = 0; i < numbrushes; i++ )
			{
				btVector3 N1 = planeEquations[i];
				if( planeEquation.dot( ref N1 ) > (double)( 0.999 ) )
				{
					return false;
				}
			}
			return true;
		}

		public static bool areVerticesBehindPlane( ref btVector3 planeNormal, btList<btVector3> vertices, double margin )
		{
			btVector3[] va = vertices.InternalArray;
			int numvertices = vertices.Count;
			for( int i = 0; i < numvertices; i++ )
			{
				//ref btVector3 N1 = vertices[i];
				double dist = (double)( planeNormal.dot( ref va[i] ) ) + (double)( planeNormal[3] ) - margin;
				if( dist > btScalar.BT_ZERO )
				{
					return false;
				}
			}
			return true;
		}

		public static void shortestArcQuat( ref btVector3 v0, ref btVector3 v1, out btQuaternion result ) // Game Programming Gems 2.10 make sure v0,v1 are normalized
		{
			btVector3 c;
			v0.cross( ref v1, out c );
			double d = v0.dot( ref v1 );

			if( d < -1.0 + btScalar.SIMD_EPSILON )
			{
				btVector3 n, unused;
				btVector3.btPlaneSpace1( ref v0, out n, out unused );
				result.x = n.x; result.y = n.y; result.z = n.z; result.w = 0.0f; // just pick any vector that is orthogonal to v0
				return;
			}

			double s = btScalar.btSqrt( ( 1.0f + d ) * 2.0f );
			double rs = 1.0f / s;
			result.x = c.x * rs;
			result.y = c.y * rs;
			result.z = c.z * rs;
			result.w = s * 0.5f;
		}

		internal int get_limit_motor_info2(
			btRotationalLimitMotor limot,
			ref btTransform transA, ref btTransform transB, ref btVector3 linVelA, ref btVector3 linVelB, ref btVector3 angVelA, ref btVector3 angVelB,
			ref btConstraintInfo2 info, int row, ref btVector3 ax1, bool rotational, bool rotAllowed = false )
		{
			//int srow = row * info.rowskip;
			bool powered = limot.m_enableMotor;
			int limit = limot.m_currentLimit;
			if( powered || limit != 0 )
			{   // if the joint is powered, or has joint limits, add in the extra row
				if( rotational )
				{
					info.m_solverConstraints[row].m_relpos1CrossNormal = ax1;
					ax1.Invert( out info.m_solverConstraints[row].m_relpos2CrossNormal );
				}
				else
				{
					info.m_solverConstraints[row].m_contactNormal1 = ax1;
					ax1.Invert( out info.m_solverConstraints[row].m_contactNormal2 );
				}
				/*
				double* J1 = rotational ? info.m_J1angularAxis : info.m_J1linearAxis;
				double* J2 = rotational ? info.m_J2angularAxis : info.m_J2linearAxis;
				J1[srow + 0] = ax1[0];
				J1[srow + 1] = ax1[1];
				J1[srow + 2] = ax1[2];

				J2[srow + 0] = -ax1[0];
				J2[srow + 1] = -ax1[1];
				J2[srow + 2] = -ax1[2];
				*/

				if( ( !rotational ) )
				{
					if( m_useOffsetForConstraintFrame )
					{
						btVector3 tmpA, tmpB, relA, relB;
						// get vector from bodyB to frameB in WCS
						m_calculatedTransformB.m_origin.Sub( ref transB.m_origin, out relB );
						// get its projection to constraint axis
						btVector3 projB = ax1 * relB.dot( ax1 );
						// get vector directed from bodyB to constraint axis (and orthogonal to it)
						btVector3 orthoB = relB - projB;
						// same for bodyA
						m_calculatedTransformA.m_origin.Sub( ref transA.m_origin, out relA );
						btVector3 projA = ax1 * relA.dot( ax1 );
						btVector3 orthoA = relA - projA;
						// get desired offset between frames A and B along constraint axis
						double desiredOffs = limot.m_currentPosition - limot.m_currentLimitError;
						// desired vector from projection of center of bodyA to projection of center of bodyB to constraint axis
						btVector3 totalDist = projA + ax1 * desiredOffs - projB;
						// get offset vectors relA and relB
						relA = orthoA + totalDist * m_factA;
						relB = orthoB - totalDist * m_factB;
						tmpA = relA.cross( ax1 );
						tmpB = relB.cross( ax1 );
						if( m_hasStaticBody && ( !rotAllowed ) )
						{
							tmpA *= m_factA;
							tmpB *= m_factB;
						}
						//int i;
						info.m_solverConstraints[row].m_relpos1CrossNormal = tmpA;
						tmpB.Invert( out info.m_solverConstraints[row].m_relpos2CrossNormal );
						//for( i = 0; i < 3; i++ ) info.m_J1angularAxis[srow + i] = tmpA[i];
						//for( i = 0; i < 3; i++ ) info.m_J2angularAxis[srow + i] = -tmpB[i];
					}
					else
					{
						btVector3 ltd;  // Linear Torque Decoupling vector
						btVector3 c = m_calculatedTransformB.m_origin - transA.m_origin;
						ltd = c.cross( ax1 );
						info.m_solverConstraints[row].m_relpos1CrossNormal = ltd;
						//info.m_J1angularAxis[srow + 0] = ltd[0];
						//info.m_J1angularAxis[srow + 1] = ltd[1];
						//info.m_J1angularAxis[srow + 2] = ltd[2];

						c = m_calculatedTransformB.m_origin - transB.m_origin;
						ltd = -c.cross( ax1 );
						info.m_solverConstraints[row].m_relpos2CrossNormal = ltd;
						//info.m_J2angularAxis[srow + 0] = ltd[0];
						//info.m_J2angularAxis[srow + 1] = ltd[1];
						//info.m_J2angularAxis[srow + 2] = ltd[2];
					}
				}
				// if we're limited low and high simultaneously, the joint motor is
				// ineffective
				if( limit != 0 && ( limot.m_loLimit == limot.m_hiLimit ) ) powered = false;
				info.m_solverConstraints[row].m_rhs = (double)( 0 );
				if( powered )
				{
					info.m_solverConstraints[row].m_cfm = limot.m_normalCFM;
					if( limit == 0 )
					{
						double tag_vel = rotational ? limot.m_targetVelocity : -limot.m_targetVelocity;

						double mot_fact = getMotorFactor( limot.m_currentPosition,
															limot.m_loLimit,
															limot.m_hiLimit,
															tag_vel,
//.........这里部分代码省略.........

		public btHingeConstraint( btRigidBody rbA, btRigidBody rbB, ref btVector3 pivotInA, ref btVector3 pivotInB,
												  ref btVector3 axisInA, ref btVector3 axisInB, bool useReferenceFrameA = false )
										 : base( btObjectTypes.HINGE_CONSTRAINT_TYPE, rbA, rbB )
		{
			Init();
#if _BT_USE_CENTER_LIMIT_
			m_limit = new btAngularLimit();
#endif
			m_useReferenceFrameA = ( useReferenceFrameA );
			m_rbAFrame.m_origin = pivotInA;

			// since no frame is given, assume this to be zero angle and just pick rb transform axis
			btVector3 rbAxisA1; rbA.m_worldTransform.m_basis.getColumn( 0, out rbAxisA1 );

			btVector3 rbAxisA2;
			double projection = axisInA.dot( rbAxisA1 );
			if( projection >= 1.0f - btScalar.SIMD_EPSILON )
			{
				btVector3 tmp;
				rbA.m_worldTransform.m_basis.getColumn( 2, out tmp );
				tmp.Invert( out rbAxisA1 );
				rbA.m_worldTransform.m_basis.getColumn( 1, out rbAxisA2 );
			}
			else if( projection <= -1.0f + btScalar.SIMD_EPSILON )
			{
				rbA.m_worldTransform.m_basis.getColumn( 2, out rbAxisA1 );
				rbA.m_worldTransform.m_basis.getColumn( 1, out rbAxisA2 );
			}
			else
			{
				axisInA.cross( ref rbAxisA1, out rbAxisA2 );
				rbAxisA2.cross( ref axisInA, out rbAxisA1 );
			}

			btMatrix3x3.setValue( out m_rbAFrame.m_basis, rbAxisA1.x, rbAxisA2.x, axisInA.x,
											rbAxisA1.y, rbAxisA2.y, axisInA.y,
											rbAxisA1.z, rbAxisA2.z, axisInA.z );

			btQuaternion rotationArc; btQuaternion.shortestArcQuat( ref axisInA, ref axisInB, out rotationArc );
			btVector3 rbAxisB1; btQuaternion.quatRotate( ref rotationArc, ref rbAxisA1, out rbAxisB1 );
			btVector3 rbAxisB2; axisInB.cross( ref rbAxisB1, out rbAxisB2 );

			m_rbBFrame.m_origin = pivotInB;
			m_rbBFrame.m_basis.setValue( ref rbAxisB1, ref rbAxisB2, ref axisInB );
			btMatrix3x3.setValue( out m_rbBFrame.m_basis, ref rbAxisB1, ref rbAxisB2, ref axisInB );

			m_referenceSign = m_useReferenceFrameA ? (double)( -1 ) : (double)( 1 );
		}

		void getInfo2InternalUsingFrameOffset( ref btConstraintInfo2 info, ref btTransform transA, ref btTransform transB, ref btVector3 angVelA, ref btVector3 angVelB )
		{
			//Debug.Assert( !m_useSolveConstraintObsolete );
			//int i;
			// transforms in world space
			btTransform trA; transA.Apply( ref m_rbAFrame, out trA );
			btTransform trB; transB.Apply( ref m_rbBFrame, out trB );
			// pivot point
			//	btVector3 pivotAInW = trA.getOrigin();
			//	btVector3 pivotBInW = trB.getOrigin();

#if true
			// difference between frames in WCS
			btVector3 ofs; trB.m_origin.Sub( ref trA.m_origin, out ofs );// getOrigin() - trA.getOrigin();
																	 // now get weight factors depending on masses
			double miA = getRigidBodyA().getInvMass();
			double miB = getRigidBodyB().getInvMass();
			bool hasStaticBody = ( miA < btScalar.SIMD_EPSILON ) || ( miB < btScalar.SIMD_EPSILON );
			double miS = miA + miB;
			double factA, factB;
			if( miS > (double)( 0 ) )
			{
				factA = miB / miS;
			}
			else
			{
				factA = (double)( 0.5f );
			}
			factB = (double)( 1.0f ) - factA;
			// get the desired direction of hinge axis
			// as weighted sum of Z-orthos of frameA and frameB in WCS
			btVector3 ax1A; trA.m_basis.getColumn( 2, out ax1A );
			btVector3 ax1B; trB.m_basis.getColumn( 2, out ax1B );
			btVector3 tmp;
			ax1A.Mult( factA, out tmp );

			btVector3 ax1; tmp.AddScale( ref ax1B, factB, out ax1 );
			ax1.normalize();
			// fill first 3 rows 
			// we want: velA + wA x relA == velB + wB x relB
			btTransform bodyA_trans = transA;
			btTransform bodyB_trans = transB;
			//int nrow = 2; // last filled row
			btVector3 tmpA, tmpB, relA, relB, p, q;
			// get vector from bodyB to frameB in WCS
			trB.m_origin.Sub( ref bodyB_trans.m_origin, out relB );
			// get its projection to hinge axis
			btVector3 projB; ax1.Mult( relB.dot( ref ax1 ), out projB );
			// get vector directed from bodyB to hinge axis (and orthogonal to it)
			btVector3 orthoB; relB.Sub( ref projB, out orthoB );
			// same for bodyA
			trA.m_origin.Sub( ref bodyA_trans.m_origin, out relA );
			btVector3 projA; ax1.Mult( relA.dot( ref ax1 ), out projA );
			btVector3 orthoA; relA.Sub( ref projA, out orthoA );
			btVector3 totalDist; projA.Sub( ref projB, out totalDist );
			// get offset vectors relA and relB
			orthoA.AddScale( ref totalDist, factA, out relA );
			orthoB.AddScale( ref totalDist, -factB, out relB );
			// now choose average ortho to hinge axis
			orthoB.Mult( factA, out tmp );
			tmp.AddScale( ref orthoA, factB, out p );
			double len2 = p.length2();
			if( len2 > btScalar.SIMD_EPSILON )
			{
				p.normalize();
			}
			else
			{
				trA.m_basis.getColumn( 1, out p );
			}
			// make one more ortho
			ax1.cross( ref p, out q );
			// fill three rows
			relA.cross( ref p, out tmpA );
			relB.cross( ref p, out tmpB );
			info.m_solverConstraints[0].m_relpos1CrossNormal = tmpA;
			tmpB.Invert( out info.m_solverConstraints[0].m_relpos2CrossNormal ); // = -tmpB;
			relA.cross( ref q, out tmpA );
			relB.cross( ref q, out tmpB );
			if( hasStaticBody && getSolveLimit() )
			{ // to make constraint between static and dynamic objects more rigid
			  // remove wA (or wB) from equation if angular limit is hit
				tmpB.Mult( factB, out tmpB );
				tmpA.Mult( factA, out tmpA );
			}
			info.m_solverConstraints[1].m_relpos1CrossNormal = tmpA;
			tmpB.Invert( out info.m_solverConstraints[1].m_relpos2CrossNormal );
			relA.cross( ref ax1, out tmpA );
			relB.cross( ref ax1, out tmpB );
			if( hasStaticBody )
			{ // to make constraint between static and dynamic objects more rigid
			  // remove wA (or wB) from equation
				tmpB.Mult( factB, out tmpB );
				tmpA.Mult( factA, out tmpA );
			}
			info.m_solverConstraints[2].m_relpos1CrossNormal = tmpA;
			tmpB.Invert( out info.m_solverConstraints[2].m_relpos2CrossNormal );

			double normalErp = ( ( m_flags & btHingeFlags.BT_HINGE_FLAGS_ERP_NORM ) != 0 ) ? m_normalERP : info.erp;
			double k = info.fps * normalErp;
//.........这里部分代码省略.........

		public override bool isInside( ref btVector3 pt, double tolerance )
		{
			btVector3 normal;
			calcNormal( out normal );
			//distance to plane
			double dist = pt.dot( ref normal );
			double planeconst = m_vertices1.dot( ref normal );
			dist -= planeconst;
			if( dist >= -tolerance && dist <= tolerance )
			{
				//inside check on edge-planes
				int i;
				for( i = 0; i < 3; i++ )
				{
					btVector3 pa, pb;
					getEdge( i, out pa, out pb );
					btVector3 edge; pb.Sub( ref pa, out edge );
					btVector3 edgeNormal; edge.cross( ref normal, out edgeNormal );
					edgeNormal.normalize();
					double dist2 = pt.dot( ref edgeNormal );
					double edgeConst = pa.dot( ref edgeNormal );
					dist2 -= edgeConst;
					if( dist2 < -tolerance )
						return false;
				}

				return true;
			}

			return false;
		}

//.........这里部分代码省略.........
					btVector3 wa = convexUtil.vertices[src];

					btVector3 wb = convexUtil.vertices[targ];
					btVector3 newEdge; wb.Sub( ref wa, out newEdge );
					newEdge.normalize();
					if( numEdges < 2 )
						edges[numEdges++] = newEdge;

					edge = edge.getNextEdgeOfFace();
				} while( edge != face );

				double planeEq = btScalar.BT_LARGE_FLOAT;


				if( numEdges == 2 )
				{
					//faceNormals[i]
					edges[0].cross( ref edges[1], out faceNormals.InternalArray[i] );
					faceNormals[i].normalize();
					tmpFaces[i].m_plane[0] = faceNormals[i].x;
					tmpFaces[i].m_plane[1] = faceNormals[i].y;
					tmpFaces[i].m_plane[2] = faceNormals[i].z;
					tmpFaces[i].m_plane[3] = planeEq;

				}
				else
				{
					Debug.Assert( false );//degenerate?
					faceNormals[i].setZero();
				}

				for( int v = 0; v < tmpFaces[i].m_indices.Count; v++ )
				{
					double eq = m_polyhedron.m_vertices[tmpFaces[i].m_indices[v]].dot( ref arr_faceNormals[i] );
					if( planeEq > eq )
					{
						planeEq = eq;
					}
				}
				tmpFaces[i].m_plane[3] = -planeEq;
			}

			//merge coplanar faces and copy them to m_polyhedron

			double faceWeldThreshold = 0.999f;
			btList<int> todoFaces = new btList<int>();
			for( int i = 0; i < tmpFaces.Count; i++ )
				todoFaces.Add( i );

			btList<int> coplanarFaceGroup = new btList<int>();
			while( todoFaces.Count > 0 )
			{
				int refFace = todoFaces[todoFaces.Count - 1];

				coplanarFaceGroup.Add( refFace );
				btConvexPolyhedron.btFace faceA = tmpFaces[refFace];
				todoFaces.Count--;

				btVector3 faceNormalA = new btVector3( faceA.m_plane[0], faceA.m_plane[1], faceA.m_plane[2] );
				for( int j = todoFaces.Count - 1; j >= 0; j-- )
				{
					int i = todoFaces[j];
					btConvexPolyhedron.btFace faceB = tmpFaces[i];
					btVector3 faceNormalB = new btVector3( faceB.m_plane[0], faceB.m_plane[1], faceB.m_plane[2] );
					if( faceNormalA.dot( ref faceNormalB ) > faceWeldThreshold )
					{

		/*@brief Return the dot product between two vectors */
		public static double btDot( ref btVector3 v1, ref btVector3 v2 )
		{
			return v1.dot( ref v2 );
		}

		/*@brief Return a rotated version of this vector
          @param wAxis The axis to rotate about 
          @param angle The angle to rotate by */
		public void rotate( ref btVector3 wAxis, double angle, out btVector3 result )
		{
			btVector3 o;
			wAxis.Mult( wAxis.dot( ref this ), out o );
			btVector3 _x;
			this.Sub( ref o, out _x );
			btVector3 _y;

			wAxis.cross( ref this, out _y );
			btVector3 tmp;
			btVector3 tmp2;
			_x.Mult( btScalar.btCos( angle ), out tmp );
			o.Add( ref tmp, out tmp2 );
			_y.Mult( btScalar.btSin( angle ), out tmp );
			tmp2.Add( ref tmp, out result );
		}

		internal void setupFrictionConstraint( btSolverConstraint solverConstraint, ref btVector3 normalAxis
			//, int solverBodyIdA, int solverBodyIdB
			, btSolverBody solverBodyA, btSolverBody solverBodyB
			, btManifoldPoint cp, ref btVector3 rel_pos1, ref btVector3 rel_pos2, btCollisionObject colObj0, btCollisionObject colObj1, double relaxation, double desiredVelocity = 0, double cfmSlip = 0.0 )
		{
			//btSolverBody solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
			//btSolverBody solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];

			btRigidBody body0 = solverBodyA.m_originalBody;
			btRigidBody body1 = solverBodyB.m_originalBody;

			solverConstraint.m_solverBodyA = solverBodyA;
			solverConstraint.m_solverBodyB = solverBodyB;

			solverConstraint.m_friction = cp.m_combinedFriction;
			solverConstraint.m_originalContactPoint = null;

			solverConstraint.m_appliedImpulse = 0;
			solverConstraint.m_appliedPushImpulse = 0;

			if( body0 != null )
			{
				solverConstraint.m_contactNormal1 = normalAxis;
				rel_pos1.cross( ref solverConstraint.m_contactNormal1, out solverConstraint.m_relpos1CrossNormal );
				btVector3 tmp;
				body0.m_invInertiaTensorWorld.Apply( ref solverConstraint.m_relpos1CrossNormal, out tmp );
				tmp.Mult( ref body0.m_angularFactor, out solverConstraint.m_angularComponentA );
			}
			else
			{
				solverConstraint.m_contactNormal1.setZero();
				solverConstraint.m_relpos1CrossNormal.setZero();
				solverConstraint.m_angularComponentA.setZero();
			}

			if( body1 != null )
			{
				normalAxis.Invert( out solverConstraint.m_contactNormal2 );
				rel_pos2.cross( ref solverConstraint.m_contactNormal2, out solverConstraint.m_relpos2CrossNormal );
				btVector3 tmp;
				body1.m_invInertiaTensorWorld.Apply( ref solverConstraint.m_relpos2CrossNormal, out tmp );
				tmp.Mult( ref body1.m_angularFactor, out solverConstraint.m_angularComponentB );
			}
			else
			{
				solverConstraint.m_contactNormal2 = btVector3.Zero;
				solverConstraint.m_relpos2CrossNormal = btVector3.Zero;
				solverConstraint.m_angularComponentB = btVector3.Zero;
			}

			{
				btVector3 vec;
				double denom0 = 0;
				double denom1 = 0;
				if( body0 != null )
				{
					solverConstraint.m_angularComponentA.cross( ref rel_pos1, out vec );
					denom0 = body0.getInvMass() + normalAxis.dot( ref vec );
				}
				if( body1 != null )
				{
					btVector3 tmp;
					solverConstraint.m_angularComponentB.Invert( out tmp );
					tmp.cross( ref rel_pos2, out vec );
					denom1 = body1.getInvMass() + normalAxis.dot( ref vec );
				}
				double denom = relaxation / ( denom0 + denom1 );
				btScalar.Dbg( "m_jacDiagABInv 1 set to " + denom.ToString( "g17" ) );
				solverConstraint.m_jacDiagABInv = denom;
			}

			{


				double rel_vel;
				double vel1Dotn;
				double vel2Dotn;
				//double vel1Dotn = solverConstraint.m_contactNormal1.dot( body0 != null ? solverBodyA.m_linearVelocity + solverBodyA.m_externalForceImpulse : btVector3.Zero )
				//	+ solverConstraint.m_relpos1CrossNormal.dot( body0 != null ? solverBodyA.m_angularVelocity : btVector3.Zero );
				if( body0 != null )
					vel1Dotn = solverConstraint.m_contactNormal1.dotAdded( ref solverBodyA.m_linearVelocity, ref solverBodyA.m_externalForceImpulse )
						+ solverConstraint.m_relpos1CrossNormal.dot( ref solverBodyA.m_angularVelocity );
				else
					vel1Dotn = 0;

				//double vel2Dotn = solverConstraint.m_contactNormal2.dot( body1 != null ? solverBodyB.m_linearVelocity + solverBodyB.m_externalForceImpulse : btVector3.Zero )
				//	+ solverConstraint.m_relpos2CrossNormal.dot( body1 != null ? solverBodyB.m_angularVelocity : btVector3.Zero );
				if( body1 != null )
					vel2Dotn = solverConstraint.m_contactNormal2.dotAdded( ref solverBodyB.m_linearVelocity, ref solverBodyB.m_externalForceImpulse )
						+ solverConstraint.m_relpos2CrossNormal.dot( ref solverBodyB.m_angularVelocity );
				else
					vel2Dotn = 0;


				rel_vel = vel1Dotn + vel2Dotn;

				//		double positionalError = 0;

				double velocityError = desiredVelocity - rel_vel;
				double velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
//.........这里部分代码省略.........

		//! apply the correction impulses for two bodies
		//double solveAngularLimits(double timeStep,ref btVector3 axis, double jacDiagABInv,btRigidBody * body0, btRigidBody * body1);
		internal double solveAngularLimits(
			double timeStep, ref btVector3 axis, double jacDiagABInv,
			btRigidBody body0, btRigidBody body1 )
		{
			if( needApplyTorques() == false ) return 0.0f;

			double target_velocity = m_targetVelocity;
			double 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

			btVector3 angVelA = body0.getAngularVelocity();
			btVector3 angVelB = body1.getAngularVelocity();

			btVector3 vel_diff;
			vel_diff = angVelA - angVelB;



			double rel_vel = axis.dot( vel_diff );

			// correction velocity
			double motor_relvel = m_limitSoftness * ( target_velocity - m_damping * rel_vel );


			if( motor_relvel < btScalar.SIMD_EPSILON && motor_relvel > -btScalar.SIMD_EPSILON )
			{
				return 0.0f;//no need for applying force
			}


			// correction impulse
			double unclippedMotorImpulse = ( 1 + m_bounce ) * motor_relvel * jacDiagABInv;

			// clip correction impulse
			double 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
			double lo = (double)( -btScalar.BT_LARGE_FLOAT );
			double hi = (double)( btScalar.BT_LARGE_FLOAT );

			double oldaccumImpulse = m_accumulatedImpulse;
			double sum = oldaccumImpulse + clippedMotorImpulse;
			m_accumulatedImpulse = sum > hi ? btScalar.BT_ZERO : sum < lo ? btScalar.BT_ZERO : sum;

			clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;

			btVector3 motorImp; axis.Mult( clippedMotorImpulse, out motorImp );

			body0.applyTorqueImpulse( ref motorImp );
			btVector3 tmp;
			motorImp.Invert( out tmp );
			body1.applyTorqueImpulse( ref tmp );

			return clippedMotorImpulse;


		}

		internal void getInfo2NonVirtual( btConstraintInfo2 info, ref btTransform transA, ref btTransform transB, ref btVector3 linVelA, ref btVector3 linVelB, double rbAinvMass, double rbBinvMass )
		{
			//btITransform m_calculatedTransformB = getCalculatedTransformB();

			//Debug.Assert( !m_useSolveConstraintObsolete );
			//int i;//, s = info.rowskip;

			double signFact = m_useLinearReferenceFrameA ? (double)( 1.0f ) : (double)( -1.0f );

			// difference between frames in WCS
			btVector3 ofs; m_calculatedTransformB.m_origin.Sub( ref m_calculatedTransformA.m_origin, out ofs );
			// now get weight factors depending on masses
			double miA = rbAinvMass;
			double miB = rbBinvMass;
			bool hasStaticBody = ( miA < btScalar.SIMD_EPSILON ) || ( miB < btScalar.SIMD_EPSILON );
			double miS = miA + miB;
			double factA, factB;
			if( miS > (double)( 0 ) )
			{
				factA = miB / miS;
			}
			else
			{
				factA = (double)( 0.5f );
			}
			factB = (double)( 1.0f ) - factA;
			btVector3 ax1, p, q;
			btVector3 ax1A = m_calculatedTransformA.m_basis.getColumn( 0 );
			btVector3 ax1B = m_calculatedTransformB.m_basis.getColumn( 0 );
			if( m_useOffsetForConstraintFrame )
			{
				// get the desired direction of slider axis
				// as weighted sum of X-orthos of frameA and frameB in WCS
				ax1 = ax1A * factA + ax1B * factB;
				ax1.normalize();
				// construct two orthos to slider axis
				btVector3.btPlaneSpace1( ref ax1, out p, out q );
			}
			else
			{ // old way - use frameA
				ax1 = m_calculatedTransformA.m_basis.getColumn( 0 );
				// get 2 orthos to slider axis (Y, Z)
				p = m_calculatedTransformA.m_basis.getColumn( 1 );
				q = m_calculatedTransformA.m_basis.getColumn( 2 );
			}
			// make rotations around these orthos equal
			// the slider axis should be the only unconstrained
			// rotational axis, the angular velocity of the two bodies perpendicular to
			// the slider axis should be equal. thus the constraint equations are
			//    p*w1 - p*w2 = 0
			//    q*w1 - q*w2 = 0
			// where p and q are unit vectors normal to the slider axis, and w1 and w2
			// are the angular velocity vectors of the two bodies.
			info.m_solverConstraints[0].m_relpos1CrossNormal = p;// m_J1angularAxis[0] = p[0];
			info.m_solverConstraints[1].m_relpos1CrossNormal = q;// m_J1angularAxis[0] = p[0];

			p.Invert( out info.m_solverConstraints[0].m_relpos2CrossNormal );
			q.Invert( out info.m_solverConstraints[1].m_relpos2CrossNormal );
			// compute the right hand side of the constraint equation. set relative
			// body velocities along p and q to bring the slider back into alignment.
			// if ax1A,ax1B are the unit length slider axes as computed from bodyA and
			// bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2).
			// if "theta" is the angle between ax1 and ax2, we need an angular velocity
			// along u to cover angle erp*theta in one step :
			//   |angular_velocity| = angle/time = erp*theta / stepsize
			//                      = (erp*fps) * theta
			//    angular_velocity  = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
			//                      = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
			// ...as ax1 and ax2 are unit length. if theta is smallish,
			// theta ~= sin(theta), so
			//    angular_velocity  = (erp*fps) * (ax1 x ax2)
			// ax1 x ax2 is in the plane space of ax1, so we project the angular
			// velocity to p and q to find the right hand side.
			//	double k = info.fps * info.erp * getSoftnessOrthoAng();
			double currERP = ( ( m_flags & btSliderFlags.BT_SLIDER_FLAGS_ERP_ORTANG ) != 0 ) ? m_softnessOrthoAng : m_softnessOrthoAng * info.erp;
			double k = info.fps * currERP;

			btVector3 u = ax1A.cross( ax1B );
			info.m_solverConstraints[0].m_rhs = k * u.dot( p );
			info.m_solverConstraints[1].m_rhs = k * u.dot( q );
			if( ( m_flags & btSliderFlags.BT_SLIDER_FLAGS_CFM_ORTANG ) != 0 )
			{
				info.m_solverConstraints[0].m_cfm = m_cfmOrthoAng;
				info.m_solverConstraints[1].m_cfm = m_cfmOrthoAng;
			}

			int nrow = 1; // last filled row
						  //int srow;
			double limit_err;
			int limit;
			bool powered;

			// next two rows. 
			// we want: velA + wA x relA == velB + wB x relB ... but this would
			// result in three equations, so we project along two orthos to the slider axis

			btTransform bodyA_trans = transA;
			btTransform bodyB_trans = transB;
			nrow++;
			//int s2 = nrow * s;
//.........这里部分代码省略.........

		internal void calculateTransforms( ref btTransform transA, ref btTransform transB )
		{
			if( m_useLinearReferenceFrameA /*|| ( !m_useSolveConstraintObsolete )*/ )
			{
				transA.Apply( ref m_frameInA, out m_calculatedTransformA );
				transB.Apply( ref m_frameInB, out m_calculatedTransformB );
			}
			else
			{
				transA.Apply( ref m_frameInA, out m_calculatedTransformB );
				transB.Apply( ref m_frameInB, out m_calculatedTransformA );
			}
			m_realPivotAInW = m_calculatedTransformA.m_origin;
			m_realPivotBInW = m_calculatedTransformB.m_origin;
			m_sliderAxis = m_calculatedTransformA.m_basis.getColumn( 0 ); // along X
			/*
			if( m_useLinearReferenceFrameA || m_useSolveConstraintObsolete )
			{
			  m_delta = m_realPivotBInW - m_realPivotAInW;
			}
			else
			*/
			{
				m_delta = m_realPivotAInW - m_realPivotBInW;
			}
			m_realPivotAInW.AddScale( ref m_sliderAxis, m_sliderAxis.dot( ref m_delta ), out m_projPivotInW );
			//m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot( ref m_delta ) * m_sliderAxis;
			btVector3 normalWorld;
			int i;
			//linear part
			for( i = 0; i < 3; i++ )
			{
				normalWorld = m_calculatedTransformA.m_basis.getColumn( i );
				m_depth[i] = m_delta.dot( normalWorld );
			}
		}