C# btVector3.Invert方法代码示例

		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,
//.........这里部分代码省略.........

		void calculateJacobi( btRotationalLimitMotor2 limot, ref btTransform transA, ref btTransform transB, btConstraintInfo2 info, int row, ref btVector3 ax1, bool rotational, bool rotAllowed )
		{

			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 );
			}

			if( !rotational )
			{
				btVector3 tmpA, tmpB, relA, relB;
				// get vector from bodyB to frameB in WCS
				m_calculatedTransformB.m_origin.Sub( ref transB.m_origin, out relB );
				// same for bodyA
				m_calculatedTransformA.m_origin.Sub( ref transA.m_origin, out relA ) ;
				relA.cross( ref ax1, out tmpA );
				relB.cross( ref ax1, out tmpB );
				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 );
			}
		}

		internal void setupRollingFrictionConstraint( btSolverConstraint solverConstraint, ref btVector3 normalAxis1
						, 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 )

		{
			btVector3 normalAxis = btVector3.Zero;


			solverConstraint.m_contactNormal1 = normalAxis;
			normalAxis.Invert( out solverConstraint.m_contactNormal2 );
			//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_combinedRollingFriction;
			solverConstraint.m_originalContactPoint = null;

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

			btVector3 iMJaA;
			btVector3 iMJaB;
			{
				btVector3 ftorqueAxis1; normalAxis1.Invert( out ftorqueAxis1 );
				solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
				if( body0 != null )
				{
					body0.m_invInertiaTensorWorld.Apply( ref ftorqueAxis1, out iMJaA );
					iMJaA.Mult( ref body0.m_angularFactor, out solverConstraint.m_angularComponentA );
				}
				else
					iMJaA = btVector3.Zero;
				//solverConstraint.m_angularComponentA = body0 != null ? body0.getInvInertiaTensorWorld() * ftorqueAxis1 * body0.getAngularFactor() : btVector3.Zero;
			}
			{
				btVector3 ftorqueAxis1 = normalAxis1;
				solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
				if( body1 != null )
				{
					body1.m_invInertiaTensorWorld.Apply( ref ftorqueAxis1, out iMJaB );
					iMJaB.Mult( ref body1.m_angularFactor, out solverConstraint.m_angularComponentB );
				}
				else
					iMJaB = btVector3.Zero;
				//solverConstraint.m_angularComponentB = body1 != null ? body1.getInvInertiaTensorWorld() * ftorqueAxis1 * body1.getAngularFactor() : btVector3.Zero;
			}


			{
				//btVector3 iMJaA = body0 != null ? body0.getInvInertiaTensorWorld() * solverConstraint.m_relpos1CrossNormal : btVector3.Zero;
				//btVector3 iMJaB = body1 != null ? body1.getInvInertiaTensorWorld() * solverConstraint.m_relpos2CrossNormal : btVector3.Zero;
				double sum = 0;
				sum += iMJaA.dot( ref solverConstraint.m_relpos1CrossNormal );
				sum += iMJaB.dot( ref solverConstraint.m_relpos2CrossNormal );
				btScalar.Dbg( "m_jacDiagABInv 2 set to " + ( btScalar.BT_ONE / sum ).ToString( "g17" ) );
				solverConstraint.m_jacDiagABInv = btScalar.BT_ONE / sum;
			}

			{


				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 );

				//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( 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;
				if( body1 != null )
					vel2Dotn = solverConstraint.m_contactNormal1.dotAdded( ref solverBodyB.m_linearVelocity, ref solverBodyB.m_externalForceImpulse )
						+ solverConstraint.m_relpos1CrossNormal.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 );
				solverConstraint.m_rhs = velocityImpulse;
				btScalar.Dbg( "Constraint 2 m_rhs " + solverConstraint.m_rhs.ToString( "g17" ) );
				solverConstraint.m_cfm = cfmSlip;
				solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
				solverConstraint.m_upperLimit = solverConstraint.m_friction;

			}
//.........这里部分代码省略.........

		public override void getPreferredPenetrationDirection( int index, out btVector3 penetrationVector )
		{
			calcNormal( out penetrationVector );
			if( index > 0 )
				penetrationVector.Invert( out penetrationVector );
		}

		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;
//.........这里部分代码省略.........

		//
		public static bool Penetration( btConvexShape shape0,
											 ref btTransform wtrs0,
											 btConvexShape shape1,
											 ref btTransform wtrs1,
											 ref btVector3 guess,
											 out sResults results,
											 bool usemargins = false )
		{
			tShape shape = new tShape();
			Initialize( shape0, ref wtrs0, shape1, ref wtrs1, out results, shape, usemargins );
			GJK gjk = new GJK();
			btVector3 tmp;
			guess.Invert( out tmp );
			GJK.eStatus._ gjk_status = gjk.Evaluate( shape, ref tmp );
			switch( gjk_status )
			{
				case GJK.eStatus._.Inside:
					{
						EPA epa = new EPA();
						EPA.eStatus._ epa_status = epa.Evaluate( gjk, ref tmp );
						if( epa_status != EPA.eStatus._.Failed )
						{
							btVector3 w0 = btVector3.Zero;
							for( uint i = 0; i < epa.m_result.rank; ++i )
							{
								shape.Support( ref epa.m_result.c[i].d, 0, out tmp );
								w0.AddScale( ref tmp, epa.m_result.p[i], out w0 );
							}
							results.status = sResults.eStatus.Penetrating;
							wtrs0.Apply( ref w0, out results.witness0 );
							w0.SubScale( ref epa.m_normal, epa.m_depth, out tmp );
							wtrs0.Apply( ref tmp, out results.witness1 );
							epa.m_normal.Invert( out results.normal );
							results.distance = -epa.m_depth;
							return ( true );
						}
						else results.status = sResults.eStatus.EPA_Failed;
					}
					break;
				case GJK.eStatus._.Failed:
					results.status = sResults.eStatus.GJK_Failed;
					break;
				default:
					break;
			}
			return ( false );
		}

			internal eStatus._ Evaluate( GJK gjk, ref btVector3 guess )
			{
				GJK.sSimplex simplex = gjk.m_simplex;
				if( ( simplex.rank > 1 ) && gjk.EncloseOrigin() )
				{

					/* Clean up				*/
					while( m_hull.root != null )
					{
						sFace f = m_hull.root;
						remove( m_hull, f );
						append( m_stock, f );
					}
					m_status = eStatus._.Valid;
					m_nextsv = 0;
					/* Orient simplex		*/
					if( btVector3.det( ref simplex.c[0].w, ref simplex.c[3].w,
						ref simplex.c[1].w, ref simplex.c[3].w,
						ref simplex.c[2].w, ref simplex.c[3].w ) < 0 )
					{
						btScalar.btSwap( ref simplex.c[0], ref simplex.c[1] );
						btScalar.btSwap( ref simplex.p[0], ref simplex.p[1] );
					}
					/* Build initial hull	*/
					sFace[] tetra = {newface(simplex.c[0],simplex.c[1],simplex.c[2],true),
						newface(simplex.c[1],simplex.c[0],simplex.c[3],true),
						newface(simplex.c[2],simplex.c[1],simplex.c[3],true),
						newface(simplex.c[0],simplex.c[2],simplex.c[3],true)};
					if( m_hull.count == 4 )
					{
						sFace best = findbest();
						sFace outer = new sFace( best );
						uint pass = 0;
						uint iterations = 0;
						bind( tetra[0], 0, tetra[1], 0 );
						bind( tetra[0], 1, tetra[2], 0 );
						bind( tetra[0], 2, tetra[3], 0 );
						bind( tetra[1], 1, tetra[3], 2 );
						bind( tetra[1], 2, tetra[2], 1 );
						bind( tetra[2], 2, tetra[3], 1 );
						m_status = eStatus._.Valid;
						for( ; iterations < EPA_MAX_ITERATIONS; ++iterations )
						{
							if( m_nextsv < EPA_MAX_VERTICES )
							{
								sHorizon horizon = new sHorizon();
								sSV w = m_sv_store[m_nextsv++];
								bool valid = true;
								best.pass = (byte)( ++pass );
								gjk.getsupport( ref best.n, w );
								double wdist = best.n.dot( ref w.w ) - best.d;
								if( wdist > EPA_ACCURACY )
								{
									for( uint j = 0; ( j < 3 ) && valid; ++j )
									{
										valid &= expand( pass, w,
											best.f[j], best.e[j],
											horizon );
									}
									if( valid & ( horizon.nf >= 3 ) )
									{
										bind( horizon.cf, 1, horizon.ff, 2 );
										remove( m_hull, best );
										append( m_stock, best );
										best = findbest();
										outer = new sFace( best );
									}
									else { m_status = eStatus._.InvalidHull; break; }
								}
								else { m_status = eStatus._.AccuraryReached; break; }
							}
							else { m_status = eStatus._.OutOfVertices; break; }
						}
						btVector3 projection; outer.n.Mult( outer.d, out projection );
						m_normal = outer.n;
						m_depth = outer.d;
						m_result.rank = 3;
						m_result.c[0] = outer.c[0];
						m_result.c[1] = outer.c[1];
						m_result.c[2] = outer.c[2];
						btVector3 tmp;
						btVector3.btCross2Del( ref outer.c[1].w, ref projection,
							ref outer.c[2].w, ref projection, out tmp );
						m_result.p[0] = tmp.length();
						btVector3.btCross2Del( ref outer.c[2].w, ref projection,
							ref outer.c[0].w, ref projection, out tmp );
						m_result.p[1] = tmp.length();
						btVector3.btCross2Del( ref outer.c[0].w, ref projection,
							ref outer.c[1].w, ref projection, out tmp );
						m_result.p[2] = tmp.length();
						double sum = m_result.p[0] + m_result.p[1] + m_result.p[2];
						m_result.p[0] /= sum;
						m_result.p[1] /= sum;
						m_result.p[2] /= sum;
						return ( m_status );
					}
				}
				/* Fallback		*/
				m_status = eStatus._.FallBack;
				guess.Invert( out m_normal );
//.........这里部分代码省略.........

		void collideSingleContact( bool usePertube, ref btQuaternion perturbeRot
			, btCollisionObjectWrapper convexObjWrap
			, ref btTransform convexTransform
			, btCollisionObjectWrapper planeObjWrap
			, ref btTransform planeTransform
			, btDispatcherInfo dispatchInfo, btManifoldResult resultOut
			, ref btVector3 planeNormal, double planeConstant
			)
		{
			//btCollisionObjectWrapper convexObjWrap = m_swapped ? body1Wrap : body0Wrap;
			//btCollisionObjectWrapper planeObjWrap = m_swapped ? body0Wrap : body1Wrap;

			btConvexShape convexShape = (btConvexShape)convexObjWrap.getCollisionShape();
			btStaticPlaneShape planeShape = (btStaticPlaneShape)planeObjWrap.getCollisionShape();

			bool hasCollision = false;
			//planeNormal = planeShape.getPlaneNormal().Copy( out planeNormal );
			//double planeConstant = planeShape.getPlaneConstant();

			btTransform convexWorldTransform = convexTransform;
			//btTransform planeWorldTransform = planeObjWrap.m_worldTransform;
			btTransform convexInPlaneTrans;
			planeTransform.inverseTimes( ref convexWorldTransform, out convexInPlaneTrans );

			if( usePertube )
			{
				//now perturbe the convex-world transform
				btMatrix3x3 perturbeMat = new btMatrix3x3( ref perturbeRot );
				btMatrix3x3 tmpPerturbe; convexWorldTransform.m_basis.Apply( ref perturbeMat, out tmpPerturbe );
				convexWorldTransform.m_basis = tmpPerturbe;
				//convexWorldTransform.getBasis() *= btMatrix3x3( perturbeRot );
			}

			btTransform planeInConvex;
			convexTransform.inverseTimes( ref planeObjWrap.m_collisionObject.m_worldTransform, out planeInConvex );

			btVector3 tmp, tmp2;
			planeNormal.Invert( out tmp );
			planeInConvex.m_basis.Apply( ref tmp, out tmp2 );
			btVector3 vtx; convexShape.localGetSupportingVertex( ref tmp2, out vtx );

			btVector3 vtxInPlane; convexInPlaneTrans.Apply( ref vtx, out vtxInPlane );
			double distance = ( planeNormal.dot( ref vtxInPlane ) - planeConstant );

			btVector3 vtxInPlaneProjected; vtxInPlane.AddScale( ref planeNormal, -distance, out vtxInPlaneProjected );
			btVector3 vtxInPlaneWorld; planeTransform.Apply( ref vtxInPlaneProjected, out vtxInPlaneWorld );

			hasCollision = distance < m_manifoldPtr.getContactBreakingThreshold();
			resultOut.setPersistentManifold( m_manifoldPtr );
			if( hasCollision )
			{
				/// report a contact. internally this will be kept persistent, and contact reduction is done
				btVector3 normalOnSurfaceB; planeTransform.m_basis.Apply( ref planeNormal, out normalOnSurfaceB );
				btScalar.Dbg( "Convex plane adds point " + normalOnSurfaceB + " " + vtxInPlaneWorld + " " + distance.ToString( "g17" ) );
				resultOut.addContactPoint( ref normalOnSurfaceB, ref vtxInPlaneWorld, distance );
			}
		}