C++ quatRotate函数代码示例

void AiCarStandard::AnalyzeOthers(float dt, const CarDynamics cars[], const int cars_num)
{
	const float half_carlength = 1.25;
	const btVector3 throttle_axis = Direction::forward;

	for (int i = 0; i != cars_num; ++i)
	{
		const CarDynamics * icar = &cars[i];
		if (icar != car)
		{
			OtherCarInfo & info = othercars[icar];

			// find direction of other cars in our frame
			btVector3 relative_position = icar->GetCenterOfMass() - car->GetCenterOfMass();
			relative_position = quatRotate(-car->GetOrientation(), relative_position);

			// only make a move if the other car is within our distance limit
			float fore_position = relative_position.dot(throttle_axis);

			btVector3 myvel = quatRotate(-car->GetOrientation(), car->GetVelocity());
			btVector3 othervel = quatRotate(-icar->GetOrientation(), icar->GetVelocity());
			float speed_diff = othervel.dot(throttle_axis) - myvel.dot(throttle_axis);

			const float fore_position_offset = -half_carlength;
			if (fore_position > fore_position_offset)
			{
				const Bezier * othercarpatch = GetCurrentPatch(icar);
				const Bezier * mycarpatch = GetCurrentPatch(car);

				if (othercarpatch && mycarpatch)
				{
					Vec3 mypos = ToMathVector<float>(car->GetCenterOfMass());
					Vec3 otpos = ToMathVector<float>(icar->GetCenterOfMass());
					float my_track_placement = GetHorizontalDistanceAlongPatch(*mycarpatch, mypos);
					float their_track_placement = GetHorizontalDistanceAlongPatch(*othercarpatch, otpos);

					float speed_diff_denom = clamp(speed_diff, -100, -0.01);
					float eta = (fore_position - fore_position_offset) / -speed_diff_denom;

					if (!info.active)
						info.eta = eta;
					else
						info.eta = RateLimit(info.eta, eta, 10.f*dt, 10000.f*dt);

					info.horizontal_distance = their_track_placement - my_track_placement;
					info.fore_distance = fore_position;
					info.active = true;
				}
				else
				{
					info.active = false;
				}
			}
			else
			{
				info.active = false;
			}
		}
	}
}

void SpaceObject::fire_laser() {
	if(weapons[activeWeapon]->ammo<600)
		return;
	btTransform trans;
	body->getMotionState()->getWorldTransform(trans);
	btVector3 from = trans.getOrigin() + quatRotate(trans.getRotation() , btVector3(0,0,-10));
	std::cout << body->getOrientation().getX() << "," << body->getOrientation().getY() << "," << body->getOrientation().getZ() << "," << body->getOrientation().getW() << "\n";
	btVector3 to = from + quatRotate(trans.getRotation() , btVector3(0,0,-500));
		
	//Perform ray test.
	btCollisionWorld::ClosestRayResultCallback rayCallback( from, to );
	world->dynamicsWorld->rayTest(from,to,rayCallback);
	//get results.
	if ( rayCallback.hasHit() ) {
		to = rayCallback.m_hitPointWorld;
		// to = ((SpaceObject*)rayCallback.m_collisionObject->getUserPointer())->getRigidBody()->getCenterOfMassPosition();
		ship_hit = (SpaceObject*)rayCallback.m_collisionObject->getUserPointer();
		if(ship_hit->getType() != SKYRISE_TALL && ship_hit->getType()!= SKYRISE_FAT && ship_hit->getType()!= ENDPOINT)
			wasHit = true;
	} //else, do nothing.
	weapons[activeWeapon] -> fireProjectile(from,to);
	fireFrom = from;
	fireTo = to;
	fired = true;
}

	btVector3 GetWheelPosition(btScalar displacement_fraction)
	{
		btVector3 up (0, 0, 1);
		btVector3 hinge_end = strut.end - strut.hinge;
		btVector3 end_top = strut.top - strut.end;
		btVector3 hinge_top = strut.top - strut.hinge;

		btVector3 rotaxis = up.cross(hinge_end).normalized();
		btVector3 localwheelpos = info.position - strut.end;

		btScalar hingeradius = hinge_end.length();
		btScalar disp_rad = asin(displacement_fraction * info.travel / hingeradius);

		btQuaternion hingerotate(rotaxis, -disp_rad);

		btScalar e_angle = angle_from_sides(end_top.length(), hinge_end.length(), hinge_top.length());

		hinge_end = quatRotate(hingerotate, hinge_end);

		btScalar e_angle_disp = angle_from_sides(end_top.length(), hinge_end.length(), hinge_top.length());

		rotaxis = up.cross(end_top).normalized();

		mountrot.setRotation(rotaxis, e_angle_disp - e_angle);
		localwheelpos = quatRotate(mountrot, localwheelpos);

		return localwheelpos + strut.hinge + hinge_end;
	}

btVector3 btMultiBody::worldDirToLocal(int i, const btVector3 &world_dir) const
{
    if (i == -1) {
        return quatRotate(getWorldToBaseRot(), world_dir);
    } else {
        return quatRotate(getParentToLocalRot(i) ,worldDirToLocal(getParent(i), world_dir));
    }
}

btVector3 btMultiBody::localDirToWorld(int i, const btVector3 &local_dir) const
{
    btVector3 result = local_dir;
    while (i != -1) {
        result = quatRotate(getParentToLocalRot(i).inverse() , result);
        i = getParent(i);
    }
    result = quatRotate(getWorldToBaseRot().inverse() , result);
    return result;
}

	btRaycastBar2 (btScalar ray_length, btScalar z,btScalar max_y)
	{
		frame_counter = 0;
		ms = 0;
		max_ms = 0;
		min_ms = 9999;
		sum_ms_samples = 0;
		sum_ms = 0;
		dx = 10.0;
		min_x = 0;
		max_x = 0;
		this->max_y = max_y;
		sign = 1.0;
		btScalar dalpha = 2*SIMD_2_PI/NUMRAYS;
		for (int i = 0; i < NUMRAYS; i++)
		{
			btScalar alpha = dalpha * i;
			// rotate around by alpha degrees y 
			btQuaternion q(btVector3(0.0, 1.0, 0.0), alpha);
			direction[i] = btVector3(1.0, 0.0, 0.0);
			direction[i] = quatRotate(q , direction[i]);
			direction[i] = direction[i] * ray_length;
			
			
			source[i] = btVector3(min_x, max_y, z);
			dest[i] = source[i] + direction[i];
			dest[i][1]=-1000;
			normal[i] = btVector3(1.0, 0.0, 0.0);
		}
	}

btVector3 btConeTwistConstraint::GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const
{
	// compute x/y in ellipse using cone angle (0 -> 2*PI along surface of cone)
	btScalar xEllipse = btCos(fAngleInRadians);
	btScalar yEllipse = btSin(fAngleInRadians);

	// Use the slope of the vector (using x/yEllipse) and find the length
	// of the line that intersects the ellipse:
	//  x^2   y^2
	//  --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits)
	//  a^2   b^2
	// Do the math and it should be clear.

	float swingLimit = m_swingSpan1; // if xEllipse == 0, just use axis b (1)
	if (fabs(xEllipse) > SIMD_EPSILON)
	{
		btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse);
		btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2);
		norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1);
		btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
		swingLimit = sqrt(swingLimit2);
	}

	// convert into point in constraint space:
	// note: twist is x-axis, swing 1 and 2 are along the z and y axes respectively
	btVector3 vSwingAxis(0, xEllipse, -yEllipse);
	btQuaternion qSwing(vSwingAxis, swingLimit);
	btVector3 vPointInConstraintSpace(fLength,0,0);
	return quatRotate(qSwing, vPointInConstraintSpace);
}

btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA)
:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false)
{

	// since no frame is given, assume this to be zero angle and just pick rb transform axis
	// fixed axis in worldspace
	btVector3 rbAxisA1, rbAxisA2;
	btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2);

	m_rbAFrame.getOrigin() = pivotInA;
	m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
									rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
									rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );

	btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * -axisInA;

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


	m_rbBFrame.getOrigin() = rbA.getCenterOfMassTransform()(pivotInA);
	m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
									rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
									rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
	
	//start with free
	m_lowerLimit = btScalar(1e30);
	m_upperLimit = btScalar(-1e30);
	m_biasFactor = 0.3f;
	m_relaxationFactor = 1.0f;
	m_limitSoftness = 0.9f;
	m_solveLimit = false;
}

btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA)
:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false), 
m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
m_useReferenceFrameA(useReferenceFrameA),
m_flags(0),m_limit()
{

	// since no frame is given, assume this to be zero angle and just pick rb transform axis
	// fixed axis in worldspace
	btVector3 rbAxisA1, rbAxisA2;
	btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2);

	m_rbAFrame.getOrigin() = pivotInA;
	m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
									rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
									rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );

	btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * axisInA;

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


	m_rbBFrame.getOrigin() = rbA.getCenterOfMassTransform()(pivotInA);
	m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
									rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
									rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
	
	m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
}

//-----------------------------------------------------------------------------
void AIBaseController::checkPosition(const Vec3 &point, posData *pos_data,
                                     Vec3 *lc, bool use_front_xyz) const
{
    // Convert to local coordinates from the point of view of current kart
    btQuaternion q(btVector3(0, 1, 0), -m_kart->getHeading());
    Vec3 p = point -
        (use_front_xyz ? m_kart->getFrontXYZ() : m_kart->getXYZ());
    Vec3 local_coordinates = quatRotate(q, p);

    // Save local coordinates for later use if needed
    if (lc) *lc = local_coordinates;

    if (pos_data == NULL) return;
    // lhs: tell whether it's left or right hand side
    if (local_coordinates.getX() < 0)
        pos_data->lhs = true;
    else
        pos_data->lhs = false;

    // behind: tell whether it's behind or not
    if (local_coordinates.getZ() < 0)
        pos_data->behind = true;
    else
        pos_data->behind = false;

    pos_data->angle = atan2(fabsf(local_coordinates.getX()),
        fabsf(local_coordinates.getZ()));
    pos_data->distance = p.length();

}   //  checkPosition

void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt)
{
    // convert target from body to constraint space
    btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * qAinB * m_rbAFrame.getRotation();
    qConstraint.normalize();

    // extract "pure" hinge component
    btVector3 vNoHinge = quatRotate(qConstraint, vHinge);
    vNoHinge.normalize();
    btQuaternion qNoHinge = shortestArcQuat(vHinge, vNoHinge);
    btQuaternion qHinge = qNoHinge.inverse() * qConstraint;
    qHinge.normalize();

    // compute angular target, clamped to limits
    btScalar targetAngle = qHinge.getAngle();
    if (targetAngle > SIMD_PI) // long way around. flip quat and recalculate.
    {
        qHinge = -(qHinge);
        targetAngle = qHinge.getAngle();
    }
    if (qHinge.getZ() < 0)
        targetAngle = -targetAngle;

    setMotorTarget(targetAngle, dt);
}

btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
									 const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA)
									 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),
#ifdef _BT_USE_CENTER_LIMIT_
									 m_limit(),
#endif
									 m_angularOnly(false),
									 m_enableAngularMotor(false),
									 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
									 m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
									 m_useReferenceFrameA(useReferenceFrameA),
									 m_flags(0),
									 m_normalCFM(0),
									 m_normalERP(0),
									 m_stopCFM(0),
									 m_stopERP(0)
{
	m_rbAFrame.getOrigin() = pivotInA;
	
	// since no frame is given, assume this to be zero angle and just pick rb transform axis
	btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0);

	btVector3 rbAxisA2;
	btScalar projection = axisInA.dot(rbAxisA1);
	if (projection >= 1.0f - SIMD_EPSILON) {
		rbAxisA1 = -rbA.getCenterOfMassTransform().getBasis().getColumn(2);
		rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
	} else if (projection <= -1.0f + SIMD_EPSILON) {
		rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(2);
		rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);      
	} else {
		rbAxisA2 = axisInA.cross(rbAxisA1);
		rbAxisA1 = rbAxisA2.cross(axisInA);
	}

	m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
									rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
									rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );

	btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
	btVector3 rbAxisB1 =  quatRotate(rotationArc,rbAxisA1);
	btVector3 rbAxisB2 =  axisInB.cross(rbAxisB1);	
	
	m_rbBFrame.getOrigin() = pivotInB;
	m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
									rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
									rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
	
#ifndef	_BT_USE_CENTER_LIMIT_
	//start with free
	m_lowerLimit = btScalar(1.0f);
	m_upperLimit = btScalar(-1.0f);
	m_biasFactor = 0.3f;
	m_relaxationFactor = 1.0f;
	m_limitSoftness = 0.9f;
	m_solveLimit = false;
#endif
	m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
}

void MyCharacterController::updateUpAxis(const glm::quat& rotation) {
    btVector3 oldUp = _currentUp;
    _currentUp = quatRotate(glmToBullet(rotation), LOCAL_UP_AXIS);
    if (!_isHovering) {
        const btScalar MIN_UP_ERROR = 0.01f;
        if (oldUp.distance(_currentUp) > MIN_UP_ERROR) {
            _rigidBody->setGravity(DEFAULT_GRAVITY * _currentUp);
        }
    }
}

void LinkState::propagateFK(LinkState *p, JointState *j)
{
    if (p == NULL && j == NULL)
    {
        rel_frame_.setIdentity();
        abs_position_.setValue(0, 0, 0);
        abs_orientation_.setValue(0, 0, 0, 1);
        abs_velocity_.setValue(0, 0, 0);
        abs_rot_velocity_.setValue(0, 0, 0);
    }
    else
    {
        assert(p);
        assert(j);

        abs_position_ =
            p->abs_position_
            + quatRotate(p->abs_orientation_, link_->origin_xyz_)
            + j->getTranslation();

        tf::Quaternion rel_or(link_->origin_rpy_[2], link_->origin_rpy_[1], link_->origin_rpy_[0]);
        abs_orientation_ = p->abs_orientation_ * j->getRotation() * rel_or;
        abs_orientation_.normalize();

        abs_velocity_ =
            p->abs_velocity_
            + cross(p->abs_rot_velocity_, link_->origin_xyz_)
            + j->getTransVelocity();

        abs_rot_velocity_ = p->abs_rot_velocity_ + quatRotate(abs_orientation_, j->getRotVelocity());


        // Computes the relative frame transform
        rel_frame_.setIdentity();
        rel_frame_ *= tf::Transform(tf::Quaternion(0,0,0), link_->origin_xyz_);
        rel_frame_ *= j->getTransform();
        rel_frame_ *= tf::Transform(tf::Quaternion(link_->origin_rpy_[2],
                                    link_->origin_rpy_[1],
                                    link_->origin_rpy_[0]));

        tf::Vector3 jo = j->getTransform().getOrigin();
    }
}

btVector3 btMultiBody::getAngularMomentum() const
{
	int num_links = getNumLinks();
    // TODO: would be better not to allocate memory here
    btAlignedObjectArray<btVector3> omega;omega.resize(num_links+1);
	btAlignedObjectArray<btVector3> vel;vel.resize(num_links+1);
    btAlignedObjectArray<btQuaternion> rot_from_world;rot_from_world.resize(num_links+1);
    compTreeLinkVelocities(&omega[0], &vel[0]);

    rot_from_world[0] = base_quat;
    btVector3 result = quatRotate(rot_from_world[0].inverse() , (base_inertia * omega[0]));

    for (int i = 0; i < num_links; ++i) {
        rot_from_world[i+1] = links[i].cached_rot_parent_to_this * rot_from_world[links[i].parent+1];
        result += (quatRotate(rot_from_world[i+1].inverse() , (links[i].inertia * omega[i+1])));
    }

    return result;
}