C++ Quat函数代码示例

//**************************************************************************
//
// when the mouse goes down, remember where. also, clear out the now
// position by putting it into start
//==========================================================================
void ArcBallCam::down(const float x, const float y)
//==========================================================================
{
	start = now * start;
	now = Quat();		// identity

	downX = x;
	downY = y;

	panX = 0;
	panY = 0;
}

    void TransformComponent::Translate(const Vec3& t)
    {
        if(m_DidRotate && m_FirstPerson) {
            Quat pitch = Quat(Vec3(1,0,0), m_FirstPersonYRot);
            Quat yaw = Quat(Vec3(0,1,0), m_FirstPersonXRot);
            m_Orientation = yaw * pitch;
        }
        if(m_FirstPerson)
            m_Position += (m_Orientation * t);
        else
            m_Position += t;
        m_Updated = true;
        m_DidMove = true;

        if(m_Parent->HasComponentType(CT_PHYSICS)) {
            btRigidBody* body = ((PhysicsComponent*)m_Parent->GetComponentByType(CT_PHYSICS))->m_Body;
            btTransform tr = body->getWorldTransform();
            tr.setOrigin(m_Position);
            body->setWorldTransform(tr);
        }
    }

Quat Quat::Normalized() const
{
#ifdef MATH_AUTOMATIC_SSE
	return Quat(vec4_normalize(q));
#else
	Quat copy = *this;
	float success = copy.Normalize();
	assume(success > 0 && "Quat::Normalized failed!");
	MARK_UNUSED(success);
	return copy;
#endif
}

// From "Uniform Random Rotations", Ken Shoemake, Graphics Gems III.
Quat Quat::unitRandom() {
    float x0 = uniformRandom();
    float r1 = sqrtf(1 - x0), 
          r2 = sqrtf(x0);
    float t1 = (float)G3D::twoPi() * uniformRandom();
    float t2 = (float)G3D::twoPi() * uniformRandom();
    float c1 = cosf(t1), 
          s1 = sinf(t1);
    float c2 = cosf(t2), 
          s2 = sinf(t2);
    return Quat(s1 * r1, c1 * r1, s2 * r2, c2 * r2);
}

// find normalize bisection of two quaternions
static Quat bisect (const Quat &v1, const Quat &v2)
{
	assert(!v1.w && !v2.w);

	Quat v = v1 + v2;
	double n = v.normSquared();

	if (n < 1.0e-5)
		return Quat(0, 0, 1);
	else
		return v * (1.0 / sqrt(n));
}

static void parseAnimKeys( Animation *anim,int type ){

	int cnt=0;
	short t_flags;
	in.sgetn( (char*)&t_flags,2 );
	in.pubseekoff( 8,ios_base::cur );
	in.sgetn( (char*)&cnt,2 );
	in.pubseekoff( 2,ios_base::cur );
	_log( "ANIM_TRACK: frames="+itoa( cnt ) );
	Vector pos,axis,scale;
	float angle;
	Quat quat;
	for( int k=0;k<cnt;++k ){
		int time;
		short flags;
		in.sgetn( (char*)&time,4 );
		in.sgetn( (char*)&flags,2 );
		float tens=0,cont=0,bias=0,ease_to=0,ease_from=0;
		if( flags & 1 ) in.sgetn( (char*)&tens,4 );
		if( flags & 2 ) in.sgetn( (char*)&cont,4 );
		if( flags & 4 ) in.sgetn( (char*)&bias,4 );
		if( flags & 8 ) in.sgetn( (char*)&ease_to,4 );
		if( flags & 16 ) in.sgetn( (char*)&ease_from,4 );
		switch( type ){
		case 0xb020:	//POS_TRACK_TAG
			in.sgetn( (char*)&pos,12 );
			if( conv ) pos=conv_tform*pos;
//			_log( "POS_KEY: time="+itoa(time)+" pos="+ftoa( pos.x )+","+ftoa( pos.y )+","+ftoa( pos.z ) );
			if( time<=anim_len ) anim->setPositionKey( time,pos );
			break;
		case 0xb021:	//ROT_TRACK_TAG
			in.sgetn( (char*)&angle,4 );
			in.sgetn( (char*)&axis,12 );
//			_log( "ROT_KEY: time="+itoa(time)+" angle="+ftoa(angle)+" axis="+ftoa(axis.x)+","+ftoa(axis.y)+","+ftoa(axis.z) );
			if( axis.length()>EPSILON ){
				if( flip_tris ) angle=-angle;
				if( conv ) axis=conv_tform.m*axis;
				quat=Quat( cosf( angle/2 ),axis.normalized()*sinf( angle/2 ) )*quat;
				quat.normalize();
			}
			if( time<=anim_len ) anim->setRotationKey( time,quat );
			break;
		case 0xb022:	//SCL_TRACK_TAG
			in.sgetn( (char*)&scale,12 );
			if( conv ) scale=conv_tform.m*scale;
//			scale.x=fabs(scale.x);scale.y=fabs(scale.y);scale.z=fabs(scale.z);
			_log( "SCL_KEY: time="+itoa(time)+" scale="+ftoa( scale.x )+","+ftoa( scale.y )+","+ftoa( scale.z ) );
			if( time<=anim_len ) anim->setScaleKey( time,scale );
			break;
		}
	}
}

Quat Quat::operator *(const Quat &r) const
{
#if defined(MATH_AUTOMATIC_SSE) && defined(MATH_SSE)
	// Best: 3.456 nsecs / 9.752 ticks, Avg: 3.721 nsecs, Worst: 3.840 nsecs
	return quat_mul_quat(q, r.q);
#else
	// Best: 12.289 nsecs / 33.216 ticks, Avg: 12.585 nsecs, Worst: 13.442 nsecs
	return Quat(x*r.w + y*r.z - z*r.y + w*r.x,
	           -x*r.z + y*r.w + z*r.x + w*r.y,
	            x*r.y - y*r.x + z*r.w + w*r.z,
	           -x*r.x - y*r.y - z*r.z + w*r.w);
#endif
}

// Quat *= Matrix 
const Quat & Quat :: operator*=(const Matrix &m)
{
	// Make sure that matrix is a rotation matrix
	assert( m.isRotation(MATH_TOLERANCE) );

	// Make Quaternion a Matrix
	Matrix mtmp(*this);

	//Multiply matrix by matrix, replace the original quaternion
	*this = Quat( mtmp * m );

	return ( *this );
}

Quat::Quat(const class Any& a) {
    *this = Quat();
    if (beginsWith(toLower(a.name()), "matrix3")) {
        *this = a;
    } else {
        a.verifyName("Quat");
        a.verifyType(Any::ARRAY);
        x = a[0];
        y = a[1];
        z = a[2];
        w = a[3];
    }
}

 DynamicsComponent::DynamicsComponent()
    : mEntity(NULL)
    , mVelocity(
       dtEntity::DynamicVec3Property::SetValueCB(this, &DynamicsComponent::SetVelocity),
       dtEntity::DynamicVec3Property::GetValueCB(this, &DynamicsComponent::GetVelocity)
    )
    , mAngularVelocity(Quat(0,0,0,1))
    , mIsMoving(false)
 {
    Register(VelocityId, &mVelocity);
    Register(AngularVelocityId, &mAngularVelocity);
    Register(AccelerationId, &mAcceleration);
 }

Quat Basis::get_quat() const {

#ifdef MATH_CHECKS
	if (!is_rotation()) {
		ERR_EXPLAIN("Basis must be normalized in order to be casted to a Quaternion. Use get_rotation_quat() or call orthonormalized() instead.");
		ERR_FAIL_V(Quat());
	}
#endif
	/* Allow getting a quaternion from an unnormalized transform */
	Basis m = *this;
	real_t trace = m.elements[0][0] + m.elements[1][1] + m.elements[2][2];
	real_t temp[4];

	if (trace > 0.0) {
		real_t s = Math::sqrt(trace + 1.0);
		temp[3] = (s * 0.5);
		s = 0.5 / s;

		temp[0] = ((m.elements[2][1] - m.elements[1][2]) * s);
		temp[1] = ((m.elements[0][2] - m.elements[2][0]) * s);
		temp[2] = ((m.elements[1][0] - m.elements[0][1]) * s);
	} else {
		int i = m.elements[0][0] < m.elements[1][1] ?
						(m.elements[1][1] < m.elements[2][2] ? 2 : 1) :
						(m.elements[0][0] < m.elements[2][2] ? 2 : 0);
		int j = (i + 1) % 3;
		int k = (i + 2) % 3;

		real_t s = Math::sqrt(m.elements[i][i] - m.elements[j][j] - m.elements[k][k] + 1.0);
		temp[i] = s * 0.5;
		s = 0.5 / s;

		temp[3] = (m.elements[k][j] - m.elements[j][k]) * s;
		temp[j] = (m.elements[j][i] + m.elements[i][j]) * s;
		temp[k] = (m.elements[k][i] + m.elements[i][k]) * s;
	}

	return Quat(temp[0], temp[1], temp[2], temp[3]);
}

Quat MUST_USE_RESULT Quat::RotateFromTo(const float3 &sourceDirection, const float3 &targetDirection)
{
	assume(sourceDirection.IsNormalized());
	assume(targetDirection.IsNormalized());
	float angle = sourceDirection.AngleBetweenNorm(targetDirection);
	assume(angle >= 0.f);
	// If sourceDirection == targetDirection, the cross product comes out zero, and normalization would fail. In that case, pick an arbitrary axis.
	float3 axis = sourceDirection.Cross(targetDirection);
	float oldLength = axis.Normalize();
	if (oldLength == 0)
		axis = float3(1, 0, 0);
	return Quat(axis, angle);
}

 TransformComponent::TransformComponent() : Component(CT_TRANSFORM), m_IsStatic(false),
                                               m_DidMove(true), m_DidRotate(true), m_DidScale(true),
                                               m_Clickable(true), m_InheritOrientation(true),
                                               m_InheritPosition(true), m_InheritScale(true), m_Simulated(false), m_Updated(false),
                                               m_FirstPersonXRot(0), m_FirstPersonYRot(0), m_FirstPerson(false), m_HasParent(false)
 {
     if(m_Position.x + m_Position.y + m_Position.z != 0.0f) m_DidMove = true;
     m_Transform = Mat4::Identity;
     m_Position = Vec3();
     m_Orientation = Quat(0.0f,1.0f,0.0f,0.0f);
     m_Scale = Vec3(1.0f,1.0f,1.0f);
     m_Updated = false;
 }

AnimationClip loadClipFromFile(Platform *platform, Skeleton *skeleton, const char *filepath) {
	Assimp::Importer importer;
	// const aiScene *scene = importer.ReadFileFromMemory(file_data.contents, file_data.size, aiProcess_MakeLeftHanded);
	const aiScene *scene = importer.ReadFile(filepath, aiProcess_MakeLeftHanded|aiProcess_Triangulate);
	if(scene == 0) {
		printf("Error: %s", importer.GetErrorString());
		printf("Couldn't load scene\n");
	}

	Assert(scene->mNumAnimations > 0);

	
	const aiAnimation *anim = scene->mAnimations[0];
	float duration_seconds = anim->mDuration / anim->mTicksPerSecond;
	AnimationChannel *channels = (AnimationChannel *)platform->alloc(sizeof(AnimationChannel) * anim->mNumChannels);
	for(u32 channel_index = 0; channel_index < anim->mNumChannels; channel_index++) {
		const aiNodeAnim *channel = anim->mChannels[channel_index];
		Assert(channel->mNumPositionKeys == channel->mNumScalingKeys);
		Assert(channel->mNumRotationKeys == channel->mNumScalingKeys);
		u32 frame_count = channel->mNumPositionKeys;
		JointPose *poses = (JointPose *)platform->alloc(sizeof(JointPose) * frame_count);
		f32 *frame_times = (f32 *)platform->alloc(sizeof(f32) * frame_count);
		for(u32 key_index = 0; key_index < frame_count; key_index++) {
			aiVectorKey pos_key = channel->mPositionKeys[key_index];
			aiQuatKey rot_key = channel->mRotationKeys[key_index];
			aiVectorKey scale_key = channel->mScalingKeys[key_index];

			frame_times[key_index] = (pos_key.mTime / anim->mDuration) * duration_seconds;

			JointPose pose = {};
			pose.translation = Vec3(pos_key.mValue.x, pos_key.mValue.y, pos_key.mValue.z);
			pose.scale = Vec3(scale_key.mValue.x, scale_key.mValue.y, scale_key.mValue.z);
			pose.rotation = Quat(rot_key.mValue.x, rot_key.mValue.y, rot_key.mValue.z, rot_key.mValue.w);
			poses[key_index] = pose;
		}

		AnimationChannel result_channel = {};
		result_channel.frame_count = frame_count;
		result_channel.frame_times = frame_times;
		result_channel.joint_index = skeleton->getJointIndexFromName(channel->mNodeName.C_Str());
		result_channel.poses = poses;
		channels[channel_index] = result_channel;
	}

	AnimationClip result = {};
	result.channel_count = anim->mNumChannels;
	result.channels = channels;
	result.duration_seconds = duration_seconds;

	return result;
}

void ConnectingBone::setRotations()
{
	//cprintf("childjoint: %f %f %f\n", m_childJoint[0], m_childJoint[1], m_childJoint[2]);
	//cprintf("parentjoint: %f %f %f\n", m_parentJoint[0], m_parentJoint[1], m_parentJoint[2]);

	float xDist = m_childJoint[0] - m_parentJoint[0];
	float yDist = m_childJoint[1] - m_parentJoint[1];
	float zDist = m_childJoint[2] - m_parentJoint[2];
	
	//cprintf("xDist: %f\n", xDist);
	//cprintf("yDist: %f\n", yDist);
	//cprintf("zDist: %f\n", zDist);

	double zero = 1.0e-3;
	if (xDist == 0){
		xDist = zero;
	}
	if (yDist == 0){
		yDist = zero;
	}
	if (zDist == 0){
		zDist = zero;
	}
	float cBoneLengthTemp = m_foldedLength;
	if (cBoneLengthTemp == 0){
		cBoneLengthTemp = zero;
	}

	double angleInDegrees = 0.0;
	angleInDegrees = 57.2957795*acos(zDist / cBoneLengthTemp);
	//cprintf("angleInDegrees: %f\n", angleInDegrees);

	if (zDist <= 0.0){
		angleInDegrees = -angleInDegrees;
	}

	if (angleInDegrees == 0){
		m_foldAngle = Vec4f(0, 0, 0, 1);
		m_foldQuat = Quat();
	}
	else {
		float rx = -yDist*zDist;
		float ry = xDist*zDist;

		// Rotate about rotation vector
		m_foldAngle = Vec4f(angleInDegrees, rx, ry, 0);
		m_foldQuat.axisToQuat(Vec3d(rx, ry, 0), angleInDegrees*3.142 / 180);
		m_foldQuat.normalize();
	}
	//cprintf("global rotations: %f %f %f %f\n", m_foldQuat[0], m_foldQuat[1], m_foldQuat[2], m_foldQuat[3]);
}