Java Transform.transform方法代码示例

import com.bulletphysics.linearmath.Transform; //导入方法依赖的package包/类
public void updateWheelTransformsWS (WheelInfo wheel, boolean interpolatedTransform) {
	wheel.raycastInfo.isInContact = false;
	Stack stack = Stack.enter();

	Transform chassisTrans = getChassisWorldTransform(stack.allocTransform());
	if (interpolatedTransform && (getRigidBody().getMotionState() != null)) {
		getRigidBody().getMotionState().getWorldTransform(chassisTrans);
	}

	wheel.raycastInfo.hardPointWS.set(wheel.chassisConnectionPointCS);
	chassisTrans.transform(wheel.raycastInfo.hardPointWS);

	wheel.raycastInfo.wheelDirectionWS.set(wheel.wheelDirectionCS);
	wheel.raycastInfo.wheelDirectionWS.mul(chassisTrans.basis);

	wheel.raycastInfo.wheelAxleWS.set(wheel.wheelAxleCS);
	wheel.raycastInfo.wheelAxleWS.mul(chassisTrans.basis);
	stack.leave();
}
 

import com.bulletphysics.linearmath.Transform; //导入方法依赖的package包/类
public void updateWheelTransformsWS(WheelInfo wheel, boolean interpolatedTransform) {
	wheel.raycastInfo.isInContact = false;

	Transform chassisTrans = getChassisWorldTransform(Stack.alloc(Transform.class));
	if (interpolatedTransform && (getRigidBody().getMotionState() != null)) {
		getRigidBody().getMotionState().getWorldTransform(chassisTrans);
	}

	wheel.raycastInfo.hardPointWS.set(wheel.chassisConnectionPointCS);
	chassisTrans.transform(wheel.raycastInfo.hardPointWS);

	wheel.raycastInfo.wheelDirectionWS.set(wheel.wheelDirectionCS);
	chassisTrans.basis.transform(wheel.raycastInfo.wheelDirectionWS);

	wheel.raycastInfo.wheelAxleWS.set(wheel.wheelAxleCS);
	chassisTrans.basis.transform(wheel.raycastInfo.wheelAxleWS);
}
 

import com.bulletphysics.linearmath.Transform; //导入方法依赖的package包/类
/**
 * Apply a transform to an AABB.
 */
public void appy_transform(Transform trans) {
	Vector3f tmp = Stack.alloc(Vector3f.class);

	Vector3f center = Stack.alloc(Vector3f.class);
	center.add(max, min);
	center.scale(0.5f);

	Vector3f extends_ = Stack.alloc(Vector3f.class);
	extends_.sub(max, center);

	// Compute new center
	trans.transform(center);

	Vector3f textends = Stack.alloc(Vector3f.class);

	trans.basis.getRow(0, tmp);
	tmp.absolute();
	textends.x = extends_.dot(tmp);

	trans.basis.getRow(1, tmp);
	tmp.absolute();
	textends.y = extends_.dot(tmp);

	trans.basis.getRow(2, tmp);
	tmp.absolute();
	textends.z = extends_.dot(tmp);

	min.sub(center, textends);
	max.add(center, textends);
}
 

import com.bulletphysics.linearmath.Transform; //导入方法依赖的package包/类
public Vector3f getAncorInA(Vector3f out) {
	Transform tmpTrans = Stack.alloc(Transform.class);

	Vector3f ancorInA = out;
	ancorInA.scaleAdd((lowerLinLimit + upperLinLimit) * 0.5f, sliderAxis, realPivotAInW);
	rbA.getCenterOfMassTransform(tmpTrans);
	tmpTrans.inverse();
	tmpTrans.transform(ancorInA);
	return ancorInA;
}
 

import com.bulletphysics.linearmath.Transform; //导入方法依赖的package包/类
@Override
public void buildJacobian () {
	appliedImpulse = 0f;
	Stack stack = Stack.enter();

	Vector3 normal = stack.allocVector3();
	normal.set(0f, 0f, 0f);

	Matrix3 tmpMat1 = stack.allocMatrix3();
	Matrix3 tmpMat2 = stack.allocMatrix3();
	Vector3 tmp1 = stack.allocVector3();
	Vector3 tmp2 = stack.allocVector3();
	Vector3 tmpVec = stack.allocVector3();

	Transform centerOfMassA = rbA.getCenterOfMassTransform(stack.allocTransform());
	Transform centerOfMassB = rbB.getCenterOfMassTransform(stack.allocTransform());

	for (int i = 0; i < 3; i++) {
		VectorUtil.setCoord(normal, i, 1f);

		tmpMat1.set(centerOfMassA.basis).transpose();
		tmpMat2.set(centerOfMassB.basis).transpose();

		tmp1.set(pivotInA);
		centerOfMassA.transform(tmp1);
		tmp1.sub(rbA.getCenterOfMassPosition(tmpVec));

		tmp2.set(pivotInB);
		centerOfMassB.transform(tmp2);
		tmp2.sub(rbB.getCenterOfMassPosition(tmpVec));

		jac[i].init(tmpMat1, tmpMat2, tmp1, tmp2, normal, rbA.getInvInertiaDiagLocal(stack.allocVector3()), rbA.getInvMass(),
			rbB.getInvInertiaDiagLocal(stack.allocVector3()), rbB.getInvMass());
		VectorUtil.setCoord(normal, i, 0f);
	}
	stack.leave();
}
 

import com.bulletphysics.linearmath.Transform; //导入方法依赖的package包/类
@Override
public void getAabbSlow(Transform trans, Vector3f minAabb, Vector3f maxAabb) {
	float margin = getMargin();
	Vector3f vec = Stack.alloc(Vector3f.class);
	Vector3f tmp1 = Stack.alloc(Vector3f.class);
	Vector3f tmp2 = Stack.alloc(Vector3f.class);
	
	for (int i=0;i<3;i++)
	{
		vec.set(0f, 0f, 0f);
		VectorUtil.setCoord(vec, i, 1f);

		MatrixUtil.transposeTransform(tmp1, vec, trans.basis);
		localGetSupportingVertex(tmp1, tmp2);
		
		trans.transform(tmp2);

		VectorUtil.setCoord(maxAabb, i, VectorUtil.getCoord(tmp2, i) + margin);

		VectorUtil.setCoord(vec, i, -1f);

		MatrixUtil.transposeTransform(tmp1, vec, trans.basis);
		localGetSupportingVertex(tmp1, tmp2);
		trans.transform(tmp2);

		VectorUtil.setCoord(minAabb, i, VectorUtil.getCoord(tmp2, i) - margin);
	}
}
 

import com.bulletphysics.linearmath.Transform; //导入方法依赖的package包/类
@Override
public void getAabb(Transform trans, Vector3f aabbMin, Vector3f aabbMax) {
	Vector3f tmp = Stack.alloc(Vector3f.class);

	Vector3f localHalfExtents = Stack.alloc(Vector3f.class);
	localHalfExtents.sub(localAabbMax, localAabbMin);
	localHalfExtents.scale(0.5f);

	Vector3f localCenter = Stack.alloc(Vector3f.class);
	localCenter.add(localAabbMax, localAabbMin);
	localCenter.scale(0.5f);

	Matrix3f abs_b = Stack.alloc(trans.basis);
	MatrixUtil.absolute(abs_b);

	Vector3f center = Stack.alloc(localCenter);
	trans.transform(center);

	Vector3f extent = Stack.alloc(Vector3f.class);
	abs_b.getRow(0, tmp);
	extent.x = tmp.dot(localHalfExtents);
	abs_b.getRow(1, tmp);
	extent.y = tmp.dot(localHalfExtents);
	abs_b.getRow(2, tmp);
	extent.z = tmp.dot(localHalfExtents);

	Vector3f margin = Stack.alloc(Vector3f.class);
	margin.set(getMargin(), getMargin(), getMargin());
	extent.add(margin);

	aabbMin.sub(center, extent);
	aabbMax.add(center, extent);
}
 

import com.bulletphysics.linearmath.Transform; //导入方法依赖的package包/类
/** getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version. */
@Override
public void getAabb (Transform trans, Vector3 aabbMin, Vector3 aabbMax) {
	Stack stack = Stack.enter();
	Vector3 localHalfExtents = stack.allocVector3();
	localHalfExtents.set(localAabbMax).sub(localAabbMin);
	localHalfExtents.scl(0.5f);
	localHalfExtents.x += getMargin();
	localHalfExtents.y += getMargin();
	localHalfExtents.z += getMargin();

	Vector3 localCenter = stack.allocVector3();
	localCenter.set(localAabbMax).add(localAabbMin);
	localCenter.scl(0.5f);

	Matrix3 abs_b = stack.alloc(trans.basis);
	MatrixUtil.absolute(abs_b);

	Vector3 center = stack.alloc(localCenter);
	trans.transform(center);

	Vector3 tmp = stack.allocVector3();

	Vector3 extent = stack.allocVector3();
	tmp.x = abs_b.val[Matrix3.M00];
	tmp.y = abs_b.val[Matrix3.M01];
	tmp.z = abs_b.val[Matrix3.M02];
	extent.x = tmp.dot(localHalfExtents);
	tmp.x = abs_b.val[Matrix3.M10];
	tmp.y = abs_b.val[Matrix3.M11];
	tmp.z = abs_b.val[Matrix3.M12];
	extent.y = tmp.dot(localHalfExtents);
	tmp.x = abs_b.val[Matrix3.M20];
	tmp.y = abs_b.val[Matrix3.M21];
	tmp.z = abs_b.val[Matrix3.M22];
	extent.z = tmp.dot(localHalfExtents);

	aabbMin.set(center).sub(extent);
	aabbMax.set(center).add(extent);
	stack.leave();
}
 

import com.bulletphysics.linearmath.Transform; //导入方法依赖的package包/类
@Override
public void solveConstraint(float timeStep) {
	Vector3f tmp = Stack.alloc(Vector3f.class);
	Vector3f tmp2 = Stack.alloc(Vector3f.class);
	Vector3f tmpVec = Stack.alloc(Vector3f.class);

	Transform centerOfMassA = rbA.getCenterOfMassTransform(Stack.alloc(Transform.class));
	Transform centerOfMassB = rbB.getCenterOfMassTransform(Stack.alloc(Transform.class));
	
	Vector3f pivotAInW = Stack.alloc(pivotInA);
	centerOfMassA.transform(pivotAInW);

	Vector3f pivotBInW = Stack.alloc(pivotInB);
	centerOfMassB.transform(pivotBInW);

	Vector3f normal = Stack.alloc(Vector3f.class);
	normal.set(0f, 0f, 0f);

	//btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
	//btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();

	for (int i = 0; i < 3; i++) {
		VectorUtil.setCoord(normal, i, 1f);
		float jacDiagABInv = 1f / jac[i].getDiagonal();

		Vector3f rel_pos1 = Stack.alloc(Vector3f.class);
		rel_pos1.sub(pivotAInW, rbA.getCenterOfMassPosition(tmpVec));
		Vector3f rel_pos2 = Stack.alloc(Vector3f.class);
		rel_pos2.sub(pivotBInW, rbB.getCenterOfMassPosition(tmpVec));
		// this jacobian entry could be re-used for all iterations

		Vector3f vel1 = rbA.getVelocityInLocalPoint(rel_pos1, Stack.alloc(Vector3f.class));
		Vector3f vel2 = rbB.getVelocityInLocalPoint(rel_pos2, Stack.alloc(Vector3f.class));
		Vector3f vel = Stack.alloc(Vector3f.class);
		vel.sub(vel1, vel2);

		float rel_vel;
		rel_vel = normal.dot(vel);

		/*
		//velocity error (first order error)
		btScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
		m_rbB.getLinearVelocity(),angvelB);
		 */

		// positional error (zeroth order error)
		tmp.sub(pivotAInW, pivotBInW);
		float depth = -tmp.dot(normal); //this is the error projected on the normal

		float impulse = depth * setting.tau / timeStep * jacDiagABInv - setting.damping * rel_vel * jacDiagABInv;

		float impulseClamp = setting.impulseClamp;
		if (impulseClamp > 0f) {
			if (impulse < -impulseClamp) {
				impulse = -impulseClamp;
			}
			if (impulse > impulseClamp) {
				impulse = impulseClamp;
			}
		}

		appliedImpulse += impulse;
		Vector3f impulse_vector = Stack.alloc(Vector3f.class);
		impulse_vector.scale(impulse, normal);
		tmp.sub(pivotAInW, rbA.getCenterOfMassPosition(tmpVec));
		rbA.applyImpulse(impulse_vector, tmp);
		tmp.negate(impulse_vector);
		tmp2.sub(pivotBInW, rbB.getCenterOfMassPosition(tmpVec));
		rbB.applyImpulse(tmp, tmp2);

		VectorUtil.setCoord(normal, i, 0f);
	}
}
 

import com.bulletphysics.linearmath.Transform; //导入方法依赖的package包/类
@Override
public void getAabb (Transform trans, Vector3 aabbMin, Vector3 aabbMax) {
	Stack stack = Stack.enter();
	Vector3 localAabbMin = bvhTriMeshShape.getLocalAabbMin(stack.allocVector3());
	Vector3 localAabbMax = bvhTriMeshShape.getLocalAabbMax(stack.allocVector3());

	Vector3 tmpLocalAabbMin = stack.allocVector3();
	Vector3 tmpLocalAabbMax = stack.allocVector3();
	VectorUtil.mul(tmpLocalAabbMin, localAabbMin, localScaling);
	VectorUtil.mul(tmpLocalAabbMax, localAabbMax, localScaling);

	localAabbMin.x = (localScaling.x >= 0f) ? tmpLocalAabbMin.x : tmpLocalAabbMax.x;
	localAabbMin.y = (localScaling.y >= 0f) ? tmpLocalAabbMin.y : tmpLocalAabbMax.y;
	localAabbMin.z = (localScaling.z >= 0f) ? tmpLocalAabbMin.z : tmpLocalAabbMax.z;
	localAabbMax.x = (localScaling.x <= 0f) ? tmpLocalAabbMin.x : tmpLocalAabbMax.x;
	localAabbMax.y = (localScaling.y <= 0f) ? tmpLocalAabbMin.y : tmpLocalAabbMax.y;
	localAabbMax.z = (localScaling.z <= 0f) ? tmpLocalAabbMin.z : tmpLocalAabbMax.z;

	Vector3 localHalfExtents = stack.allocVector3();
	localHalfExtents.set(localAabbMax).sub(localAabbMin);
	localHalfExtents.scl(0.5f);

	float margin = bvhTriMeshShape.getMargin();
	localHalfExtents.x += margin;
	localHalfExtents.y += margin;
	localHalfExtents.z += margin;

	Vector3 localCenter = stack.allocVector3();
	localCenter.set(localAabbMax).add(localAabbMin);
	localCenter.scl(0.5f);

	Matrix3 abs_b = stack.alloc(trans.basis);
	MatrixUtil.absolute(abs_b);

	Vector3 center = stack.alloc(localCenter);
	trans.transform(center);

	Vector3 extent = stack.allocVector3();
	Vector3 tmp = stack.allocVector3();
	MatrixUtil.getRow(abs_b, 0, tmp);
	extent.x = tmp.dot(localHalfExtents);
	MatrixUtil.getRow(abs_b, 1, tmp);
	extent.y = tmp.dot(localHalfExtents);
	MatrixUtil.getRow(abs_b, 2, tmp);
	extent.z = tmp.dot(localHalfExtents);

	aabbMin.set(center).sub(extent);
	aabbMax.set(center).add(extent);
	stack.leave();
}
 

import com.bulletphysics.linearmath.Transform; //导入方法依赖的package包/类
@Override
public void processCollision (CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo,
	ManifoldResult resultOut) {
	if (manifoldPtr == null) {
		return;
	}

	Stack stack = Stack.enter();
	Transform tmpTrans = stack.allocTransform();

	CollisionObject convexObj = isSwapped ? body1 : body0;
	CollisionObject planeObj = isSwapped ? body0 : body1;

	ConvexShape convexShape = (ConvexShape)convexObj.getCollisionShape();
	StaticPlaneShape planeShape = (StaticPlaneShape)planeObj.getCollisionShape();

	boolean hasCollision = false;
	Vector3 planeNormal = planeShape.getPlaneNormal(stack.allocVector3());
	float planeConstant = planeShape.getPlaneConstant();

	Transform planeInConvex = stack.allocTransform();
	convexObj.getWorldTransform(planeInConvex);
	planeInConvex.inverse();
	planeInConvex.mul(planeObj.getWorldTransform(tmpTrans));

	Transform convexInPlaneTrans = stack.allocTransform();
	convexInPlaneTrans.inverse(planeObj.getWorldTransform(tmpTrans));
	convexInPlaneTrans.mul(convexObj.getWorldTransform(tmpTrans));

	Vector3 tmp = stack.allocVector3();
	tmp.set(planeNormal).scl(-1);
	tmp.mul(planeInConvex.basis);

	Vector3 vtx = convexShape.localGetSupportingVertex(tmp, stack.allocVector3());
	Vector3 vtxInPlane = stack.alloc(vtx);
	convexInPlaneTrans.transform(vtxInPlane);

	float distance = (planeNormal.dot(vtxInPlane) - planeConstant);

	Vector3 vtxInPlaneProjected = stack.allocVector3();
	tmp.set(planeNormal).scl(distance);
	vtxInPlaneProjected.set(vtxInPlane).sub(tmp);

	Vector3 vtxInPlaneWorld = stack.alloc(vtxInPlaneProjected);
	planeObj.getWorldTransform(tmpTrans).transform(vtxInPlaneWorld);

	hasCollision = distance < manifoldPtr.getContactBreakingThreshold();
	resultOut.setPersistentManifold(manifoldPtr);
	if (hasCollision) {
		// report a contact. internally this will be kept persistent, and contact reduction is done
		Vector3 normalOnSurfaceB = stack.alloc(planeNormal);
		normalOnSurfaceB.mul(planeObj.getWorldTransform(tmpTrans).basis);

		Vector3 pOnB = stack.alloc(vtxInPlaneWorld);
		resultOut.addContactPoint(normalOnSurfaceB, pOnB, distance);
	}
	if (ownManifold) {
		if (manifoldPtr.getNumContacts() != 0) {
			resultOut.refreshContactPoints();
		}
	}
	stack.leave();
}
 

import com.bulletphysics.linearmath.Transform; //导入方法依赖的package包/类
public void processTriangle(Vector3[] triangle, int partId, int triangleIndex) {
	// just for debugging purposes
	//printf("triangle %d",m_triangleCount++);

	// aabb filter is already applied!	

	ci.dispatcher1 = dispatcher;

	CollisionObject ob = (CollisionObject) triBody;

	// debug drawing of the overlapping triangles
	if (dispatchInfoPtr != null && dispatchInfoPtr.debugDraw != null && dispatchInfoPtr.debugDraw.getDebugMode() > 0) {
	    Stack stack = Stack.enter();
		Vector3 color = stack.allocVector3();
		color.set(255, 255, 0);
		Transform tr = ob.getWorldTransform(stack.allocTransform());

		Vector3 tmp1 = stack.allocVector3();
		Vector3 tmp2 = stack.allocVector3();

		tmp1.set(triangle[0]); tr.transform(tmp1);
		tmp2.set(triangle[1]); tr.transform(tmp2);
		dispatchInfoPtr.debugDraw.drawLine(tmp1, tmp2, color);

		tmp1.set(triangle[1]); tr.transform(tmp1);
		tmp2.set(triangle[2]); tr.transform(tmp2);
		dispatchInfoPtr.debugDraw.drawLine(tmp1, tmp2, color);

		tmp1.set(triangle[2]); tr.transform(tmp1);
		tmp2.set(triangle[0]); tr.transform(tmp2);
		dispatchInfoPtr.debugDraw.drawLine(tmp1, tmp2, color);

		//btVector3 center = triangle[0] + triangle[1]+triangle[2];
		//center *= btScalar(0.333333);
		//m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(center),color);
		//m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(center),color);
		//m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(center),color);
		stack.leave();
	}

	//btCollisionObject* colObj = static_cast<btCollisionObject*>(m_convexProxy->m_clientObject);

	if (convexBody.getCollisionShape().isConvex()) {
		tm.init(triangle[0], triangle[1], triangle[2]);
		tm.setMargin(collisionMarginTriangle);

		CollisionShape tmpShape = ob.getCollisionShape();
		ob.internalSetTemporaryCollisionShape(tm);

		CollisionAlgorithm colAlgo = ci.dispatcher1.findAlgorithm(convexBody, triBody, manifoldPtr);
		// this should use the btDispatcher, so the actual registered algorithm is used
		//		btConvexConvexAlgorithm cvxcvxalgo(m_manifoldPtr,ci,m_convexBody,m_triBody);

		resultOut.setShapeIdentifiers(-1, -1, partId, triangleIndex);
		//cvxcvxalgo.setShapeIdentifiers(-1,-1,partId,triangleIndex);
		//cvxcvxalgo.processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
		colAlgo.processCollision(convexBody, triBody, dispatchInfoPtr, resultOut);
		//colAlgo.destroy();
		ci.dispatcher1.freeCollisionAlgorithm(colAlgo);
		ob.internalSetTemporaryCollisionShape(tmpShape);
	}
}
 

import com.bulletphysics.linearmath.Transform; //导入方法依赖的package包/类
public void applyTransform(Transform t) {
	t.transform(vertices1[0]);
	t.transform(vertices1[1]);
	t.transform(vertices1[2]);
}
 

import com.bulletphysics.linearmath.Transform; //导入方法依赖的package包/类
@Override
public void solveConstraint (float timeStep) {
	Stack stack = Stack.enter();
	Vector3 tmp = stack.allocVector3();
	Vector3 tmp2 = stack.allocVector3();
	Vector3 tmpVec = stack.allocVector3();

	Transform centerOfMassA = rbA.getCenterOfMassTransform(stack.allocTransform());
	Transform centerOfMassB = rbB.getCenterOfMassTransform(stack.allocTransform());

	Vector3 pivotAInW = stack.alloc(pivotInA);
	centerOfMassA.transform(pivotAInW);

	Vector3 pivotBInW = stack.alloc(pivotInB);
	centerOfMassB.transform(pivotBInW);

	Vector3 normal = stack.allocVector3();
	normal.set(0f, 0f, 0f);

	// btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
	// btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();

	for (int i = 0; i < 3; i++) {
		VectorUtil.setCoord(normal, i, 1f);
		float jacDiagABInv = 1f / jac[i].getDiagonal();

		Vector3 rel_pos1 = stack.allocVector3();
		rel_pos1.set(pivotAInW).sub(rbA.getCenterOfMassPosition(tmpVec));
		Vector3 rel_pos2 = stack.allocVector3();
		rel_pos2.set(pivotBInW).sub(rbB.getCenterOfMassPosition(tmpVec));
		// this jacobian entry could be re-used for all iterations

		Vector3 vel1 = rbA.getVelocityInLocalPoint(rel_pos1, stack.allocVector3());
		Vector3 vel2 = rbB.getVelocityInLocalPoint(rel_pos2, stack.allocVector3());
		Vector3 vel = stack.allocVector3();
		vel.set(vel1).sub(vel2);

		float rel_vel;
		rel_vel = normal.dot(vel);

		/*
		 * //velocity error (first order error) btScalar rel_vel =
		 * m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, m_rbB.getLinearVelocity(),angvelB);
		 */

		// positional error (zeroth order error)
		tmp.set(pivotAInW).sub(pivotBInW);
		float depth = -tmp.dot(normal); // this is the error projected on the normal

		float impulse = depth * setting.tau / timeStep * jacDiagABInv - setting.damping * rel_vel * jacDiagABInv;

		float impulseClamp = setting.impulseClamp;
		if (impulseClamp > 0f) {
			if (impulse < -impulseClamp) {
				impulse = -impulseClamp;
			}
			if (impulse > impulseClamp) {
				impulse = impulseClamp;
			}
		}

		appliedImpulse += impulse;
		Vector3 impulse_vector = stack.allocVector3();
		impulse_vector.set(normal).scl(impulse);
		tmp.set(pivotAInW).sub(rbA.getCenterOfMassPosition(tmpVec));
		rbA.applyImpulse(impulse_vector, tmp);
		tmp.set(impulse_vector).scl(-1);
		tmp2.set(pivotBInW).sub(rbB.getCenterOfMassPosition(tmpVec));
		rbB.applyImpulse(tmp, tmp2);

		VectorUtil.setCoord(normal, i, 0f);
	}
	stack.leave();
}
 

import com.bulletphysics.linearmath.Transform; //导入方法依赖的package包/类
public HingeConstraint (RigidBody rbA, Vector3 pivotInA, Vector3 axisInA) {
	super(TypedConstraintType.HINGE_CONSTRAINT_TYPE, rbA);
	angularOnly = false;
	enableAngularMotor = false;
	Stack stack = Stack.enter();
	// since no frame is given, assume this to be zero angle and just pick rb transform axis
	// fixed axis in worldspace
	Vector3 rbAxisA1 = stack.allocVector3();
	Transform centerOfMassA = rbA.getCenterOfMassTransform(stack.allocTransform());
	MatrixUtil.getColumn(centerOfMassA.basis, 0, rbAxisA1);

	float projection = rbAxisA1.dot(axisInA);
	if (projection > BulletGlobals.FLT_EPSILON) {
		rbAxisA1.scl(projection);
		rbAxisA1.sub(axisInA);
	} else {
		MatrixUtil.getColumn(centerOfMassA.basis, 1, rbAxisA1);
	}

	Vector3 rbAxisA2 = stack.allocVector3();
	rbAxisA2.set(axisInA).crs(rbAxisA1);

	rbAFrame.origin.set(pivotInA);
	MatrixUtil.setRow(rbAFrame.basis, 0, rbAxisA1.x, rbAxisA2.x, axisInA.x);
	MatrixUtil.setRow(rbAFrame.basis, 1, rbAxisA1.y, rbAxisA2.y, axisInA.y);
	MatrixUtil.setRow(rbAFrame.basis, 2, rbAxisA1.z, rbAxisA2.z, axisInA.z);

	Vector3 axisInB = stack.allocVector3();
	axisInB.set(axisInA).scl(-1);
	axisInB.mul(centerOfMassA.basis);

	Quaternion rotationArc = QuaternionUtil.shortestArcQuat(axisInA, axisInB, stack.allocQuaternion());
	Vector3 rbAxisB1 = QuaternionUtil.quatRotate(rotationArc, rbAxisA1, stack.allocVector3());
	Vector3 rbAxisB2 = stack.allocVector3();
	rbAxisB2.set(axisInB).crs(rbAxisB1);

	rbBFrame.origin.set(pivotInA);
	centerOfMassA.transform(rbBFrame.origin);
	MatrixUtil.setRow(rbBFrame.basis, 0, rbAxisB1.x, rbAxisB2.x, axisInB.x);
	MatrixUtil.setRow(rbBFrame.basis, 1, rbAxisB1.y, rbAxisB2.y, axisInB.y);
	MatrixUtil.setRow(rbBFrame.basis, 2, rbAxisB1.z, rbAxisB2.z, axisInB.z);

	// start with free
	lowerLimit = 1e30f;
	upperLimit = -1e30f;
	biasFactor = 0.3f;
	relaxationFactor = 1.0f;
	limitSoftness = 0.9f;
	solveLimit = false;
	stack.leave();
}