Example #1
0
void Lua_V2::WorldToScreen() {
	lua_Object xObj = lua_getparam(1);
	lua_Object yObj = lua_getparam(2);
	lua_Object zObj = lua_getparam(3);
	if (!lua_isnumber(xObj) || !lua_isnumber(yObj) || !lua_isnumber(zObj)) {
		lua_pushnumber(0.0);
		lua_pushnumber(0.0);
		return;
	}

	float x = lua_getnumber(xObj);
	float y = lua_getnumber(yObj);
	float z = lua_getnumber(zObj);
	Math::Vector3d pos = Math::Vector3d(x, y, z);

	const Set::Setup *setup = g_emi->getCurrSet()->getCurrSetup();
	const Math::Vector3d interest = setup->_interest;
	const float roll = setup->_roll;
	const Math::Quaternion quat = Math::Quaternion(interest.x(), interest.y(), interest.z(), roll);
	Math::Matrix4 view = quat.toMatrix();
	view.transpose();

	pos -= setup->_pos;
	pos = view.getRotation() * pos;
	pos.z() = -pos.z();

	Math::Matrix4 proj = GfxBase::makeProjMatrix(setup->_fov, setup->_nclip, setup->_fclip);
	proj.transpose();
	Math::Vector4d screen = proj * Math::Vector4d(pos.x(), pos.y(), pos.z(), 1.0);
	screen /= screen.w();

	lua_pushnumber((screen.x() + 1) * 320);
	lua_pushnumber((1 - screen.y()) * 240);
}
Example #2
0
TEST_F(QuaternionTest, default_quaternion_is_identity_quaternion)
{
    const Math::Quaternion<double> quat;

    EXPECT_EQ(1, quat.w());
    EXPECT_EQ(0, quat.x());
    EXPECT_EQ(0, quat.y());
    EXPECT_EQ(0, quat.z());
}
Example #3
0
Math::Quaternion<double> create_quaternion_from_small_real_component(const Math::Matrix4d& matrix)
{
    Math::Quaternion<double> result;
    result.w() = 0.5 * std::sqrt(matrix_trace(matrix));
    result.x() = 0.5 * std::sqrt(matrix(0,0) - matrix(1,1) - matrix(2,2) + matrix(3,3));
    result.y() = 0.5 * std::sqrt(-matrix(0,0) + matrix(1,1) - matrix(2,2) + matrix(3,3));
    result.z() = 0.5 * std::sqrt(-matrix(0,0) - matrix(1,1) + matrix(2,2) + matrix(3,3));

    return result;
}
rendering::ShadowCameraTransform rendering::lighting::DirectionalLight::CalcShadowCameraTransform(const math::Vector3D& cameraPos, const math::Quaternion& cameraRot) const
{
	//return BaseLight::CalcShadowCameraTransform(cameraPos, cameraRot);

	// This function in directional light allows the directional light to be casting shadows only in the area around the camera current position
	ShadowCameraTransform shadowCameraTransform(cameraPos + cameraRot.GetForward() * m_halfShadowArea, GetTransform().GetTransformedRot());

	/**
	 * The reoccurring shimmering is caused by the moving shadow camera by the value less than
	 * the shadow map texel size. If I move by, let's say, half the texel size, render the scene
	 * and generate the shadow map again, the objects aren't going to map to exactly the same texels
	 * in the shadow map. This causes approximation to be calculated slightly differently each frame.
	 * To fix the shimmering effect we have to make sure we only move by the multiple of the texel size.
	 */
	/* ==================== Fixing the shimmering effect begin ==================== */
	const auto shadowMapSize = static_cast<math::Real>(1 << GetShadowInfo()->GetShadowMapSizeAsPowerOf2());
	const auto worldSpaceShadowMapTexelSize = m_halfShadowArea * 2.0f / shadowMapSize;
	// Now we transform from the world space into the light space
	auto lightSpaceCameraPos(shadowCameraTransform.pos.Rotate(shadowCameraTransform.rot.Conjugate()));

	// Now we need to snap the lightSpaceCameraPos to shadow map texel size increments
	lightSpaceCameraPos.x = worldSpaceShadowMapTexelSize * math::Floor(lightSpaceCameraPos.x / worldSpaceShadowMapTexelSize);
	lightSpaceCameraPos.y = worldSpaceShadowMapTexelSize * math::Floor(lightSpaceCameraPos.y / worldSpaceShadowMapTexelSize);

	// Now we transform back from the light space into the world space
	shadowCameraTransform.pos = lightSpaceCameraPos.Rotate(shadowCameraTransform.rot);
	/* ==================== Fixing the shimmering effect end ==================== */
	return shadowCameraTransform;
}
Example #5
0
Math::Quaternion<double> create_quaternion_from_large_real_component(const Math::Matrix4d& matrix)
{
    Math::Quaternion<double> result;
    result.w() = 0.5 * std::sqrt(matrix_trace(matrix));
    result.x() = 0.25 *(matrix(2,1) - matrix(1,2))/result.w();
    result.y() = 0.25 *(matrix(0,2) - matrix(2,0))/result.w();
    result.z() = 0.25 *(matrix(1,0) - matrix(0,1))/result.w();

    return result;
}
Example #6
0
    void MotionEmuThread() {
        auto update_time = std::chrono::steady_clock::now();
        Math::Quaternion<float> q = MakeQuaternion(Math::Vec3<float>(), 0);
        Math::Quaternion<float> old_q;

        while (!shutdown_event.WaitUntil(update_time)) {
            update_time += update_duration;
            old_q = q;

            {
                std::lock_guard<std::mutex> guard(tilt_mutex);

                // Find the quaternion describing current 3DS tilting
                q = MakeQuaternion(Math::MakeVec(-tilt_direction.y, 0.0f, tilt_direction.x),
                                   tilt_angle);
            }

            auto inv_q = q.Inverse();

            // Set the gravity vector in world space
            auto gravity = Math::MakeVec(0.0f, -1.0f, 0.0f);

            // Find the angular rate vector in world space
            auto angular_rate = ((q - old_q) * inv_q).xyz * 2;
            angular_rate *= 1000 / update_millisecond / MathUtil::PI * 180;

            // Transform the two vectors from world space to 3DS space
            gravity = QuaternionRotate(inv_q, gravity);
            angular_rate = QuaternionRotate(inv_q, angular_rate);

            // Update the sensor state
            {
                std::lock_guard<std::mutex> guard(status_mutex);
                status = std::make_tuple(gravity, angular_rate);
            }
        }
    }
Example #7
0
TEST_F(QuaternionTest, multiplying_quaternions_with_only_real_part_gives_a_quaternion_with_only_the_real_parts_multiplied_and_no_imaginary_part)
{
    Math::Quaternion<double> left;
    Math::Quaternion<double> right;

    left.w() = create_random_scalar();
    right.w() = create_random_scalar();

    auto res = left * right;

    EXPECT_EQ(left.w() * right.w(), res.w());
    EXPECT_EQ(0, res.x());
    EXPECT_EQ(0, res.y());
    EXPECT_EQ(0, res.z());
}
Example #8
0
Math::Matrix4d make_matrix_from_quaternion(const Math::Quaternion<double>& quat)
{
    Math::Matrix4d matrix;
    const auto s = 2.0 / quaternion_norm(quat);

    matrix(0,0) -= s *(quat.y() * quat.y() + quat.z() * quat.z());
    matrix(0,1) += s *(quat.x() * quat.y() - quat.w() * quat.z());
    matrix(0,2) += s *(quat.x() * quat.z() + quat.w() * quat.y());

    matrix(1,0) += s *(quat.x() * quat.y() + quat.w() * quat.z());
    matrix(1,1) -= s *(quat.x() * quat.x() + quat.z() * quat.z());
    matrix(1,2) += s *(quat.y() * quat.z() - quat.w() * quat.x());

    matrix(2,0) += s *(quat.x() * quat.z() - quat.w() * quat.y());
    matrix(2,1) += s *(quat.y() * quat.z() + quat.w() * quat.x());
    matrix(2,2) -= s *(quat.x() * quat.x() + quat.y() * quat.y());

    return matrix;
}
Example #9
0
void EMIHead::lookAt(bool entering, const Math::Vector3d &point, float rate, const Math::Matrix4 &matrix) {
	if (!_cost->_emiSkel || !_cost->_emiSkel->_obj)
		return;

	if (_jointName.empty())
		return;

	Joint *joint = _cost->_emiSkel->_obj->getJointNamed(_jointName);
	if (!joint)
		return;

	Math::Quaternion lookAtQuat; // Note: Identity if not looking at anything.

	if (entering) {
		Math::Matrix4 jointToWorld = _cost->getOwner()->getFinalMatrix() * joint->_finalMatrix;
		Math::Vector3d jointWorldPos = jointToWorld.getPosition();
		Math::Matrix4 worldToJoint = jointToWorld;
		worldToJoint.invertAffineOrthonormal();

		Math::Vector3d targetDir = (point + _offset) - jointWorldPos;
		targetDir.normalize();

		const Math::Vector3d worldUp(0, 1, 0);
		Math::Vector3d frontDir = Math::Vector3d(worldToJoint(0, 1), worldToJoint(1, 1), worldToJoint(2, 1)); // Look straight ahead. (+Y)
		Math::Vector3d modelFront(0, 0, 1);
		Math::Vector3d modelUp(0, 1, 0);

		joint->_absMatrix.inverseRotate(&modelFront);
		joint->_absMatrix.inverseRotate(&modelUp);

		// Generate a world-space look at matrix.
		Math::Matrix4 lookAtTM;
		lookAtTM.setToIdentity();

		if (Math::Vector3d::dotProduct(targetDir, worldUp) >= 0.98f) // Avoid singularity if trying to look straight up.
			lookAtTM.buildFromTargetDir(modelFront, targetDir, modelUp, -frontDir); // Instead of orienting head towards scene up, orient head towards character "back",
		else if (Math::Vector3d::dotProduct(targetDir, worldUp) <= -0.98f) // Avoid singularity if trying to look straight down.
			lookAtTM.buildFromTargetDir(modelFront, targetDir, modelUp, frontDir); // Instead of orienting head towards scene down, orient head towards character "front",
		else
			lookAtTM.buildFromTargetDir(modelFront, targetDir, modelUp, worldUp);

		// Convert from world-space to joint-space.
		lookAtTM = worldToJoint * lookAtTM;

		// Apply angle limits.
		Math::Angle p, y, r;
		lookAtTM.getXYZ(&y, &p, &r, Math::EO_ZXY);

		y.clampDegrees(_yawRange);
		p.clampDegrees(_minPitch, _maxPitch);
		r.clampDegrees(30.0f);

		lookAtTM.buildFromXYZ(y, p, r, Math::EO_ZXY);

		lookAtQuat.fromMatrix(lookAtTM.getRotation());
	}

	if (_headRot != lookAtQuat) {
		Math::Quaternion diff = _headRot.inverse() * lookAtQuat;
		float angle = 2 * acos(diff.w());
		if (diff.w() < 0.0f) {
			angle = 2 * (float)M_PI - angle;
		}

		float turnAmount = g_grim->getPerSecond(rate * ((float)M_PI / 180.0f));
		if (turnAmount < angle)
			_headRot = _headRot.slerpQuat(lookAtQuat, turnAmount / angle);
		else
			_headRot = lookAtQuat;
	}

	if (_headRot != Math::Quaternion()) { // If not identity..
		joint->_animMatrix = joint->_animMatrix * _headRot.toMatrix();
		joint->_animQuat = joint->_animQuat * _headRot;
		_cost->_emiSkel->_obj->commitAnim();
	}
}
Example #10
0
void AnimationEmi::animate(Skeleton *skel, float delta) {
	_time += delta;
	if (_time > _duration) {
		_time = _duration;
	}

	for (int bone = 0; bone < _numBones; ++bone) {
		Bone &curBone = _bones[bone];
		if (!curBone._target)
			curBone._target = skel->getJointNamed(curBone._boneName);

		Math::Matrix4 &relFinal = curBone._target->_finalMatrix;
		Math::Quaternion &quatFinal = curBone._target->_finalQuat;

		if (curBone._rotations) {
			int keyfIdx = 0;
			Math::Quaternion quat;
			Math::Vector3d relPos = relFinal.getPosition();

			for (int curKeyFrame = 0; curKeyFrame < curBone._count; curKeyFrame++) {
				if (curBone._rotations[curKeyFrame]._time >= _time) {
					keyfIdx = curKeyFrame;
					break;
				}
			}

			if (keyfIdx == 0) {
				quat = curBone._rotations[keyfIdx]._quat;
			} else if (keyfIdx == curBone._count - 1) {
				quat = curBone._rotations[keyfIdx - 1]._quat;
			} else {
				float timeDelta = curBone._rotations[keyfIdx - 1]._time - curBone._rotations[keyfIdx]._time;
				float interpVal = (_time - curBone._rotations[keyfIdx]._time) / timeDelta;

				// Might be the other way around (keyfIdx - 1 slerped against keyfIdx)
				quat = curBone._rotations[keyfIdx]._quat.slerpQuat(curBone._rotations[keyfIdx - 1]._quat, interpVal);
			}
			quat.toMatrix(relFinal);
			quatFinal = quat;
			relFinal.setPosition(relPos);
		}

		if (curBone._translations) {
			int keyfIdx = 0;
			Math::Vector3d vec;

			for (int curKeyFrame = 0; curKeyFrame < curBone._count; curKeyFrame++) {
				if (curBone._translations[curKeyFrame]._time >= _time) {
					keyfIdx = curKeyFrame;
					break;
				}
			}

			if (keyfIdx == 0) {
				vec = curBone._translations[keyfIdx]._vec;
			} else if (keyfIdx == curBone._count - 1) {
				vec = curBone._translations[keyfIdx - 1]._vec;
			} else {
				float timeDelta = curBone._translations[keyfIdx - 1]._time - curBone._translations[keyfIdx]._time;
				float interpVal = (_time - curBone._translations[keyfIdx]._time) / timeDelta;

				vec.x() = curBone._translations[keyfIdx - 1]._vec.x() +
						  (curBone._translations[keyfIdx]._vec.x() - curBone._translations[keyfIdx - 1]._vec.x()) * interpVal;

				vec.y() = curBone._translations[keyfIdx - 1]._vec.y() +
						  (curBone._translations[keyfIdx]._vec.y() - curBone._translations[keyfIdx - 1]._vec.y()) * interpVal;

				vec.z() = curBone._translations[keyfIdx - 1]._vec.z() +
						  (curBone._translations[keyfIdx]._vec.z() - curBone._translations[keyfIdx - 1]._vec.z()) * interpVal;
			}
			relFinal.setPosition(vec);
		}
	}

}
Example #11
0
void OnlineRotHec::addMeasurement( const Math::Quaternion& q, const Math::Quaternion& r )
{
	// make sure the signs of both w's are equal
	const double nq = q.w() < 0 ? -1 : 1;
	const double nr = r.w() < 0 ? -1 : 1;
	
	Math::ErrorVector< double, 3 > kalmanMeasurement;
	kalmanMeasurement.value( 0 ) = r.x() * nr - q.x() * nq;
	kalmanMeasurement.value( 1 ) = r.y() * nr - q.y() * nq;
	kalmanMeasurement.value( 2 ) = r.z() * nr - q.z() * nq;
	kalmanMeasurement.covariance = Math::Matrix< double, 3, 3 >::identity();

	// do the filter update
	Math::Matrix< double, 3, 3 > h;
	skewMatrix( h, Math::Vector< double, 3 >( q.x() * nq + r.x() * nr, q.y() * nq + r.y() * nr, q.z() * nq + r.z() * nr ) );
	Tracking::kalmanMeasurementUpdate< 3, 3 >( m_state, Math::Function::LinearFunction< 3, 3, double >( h ), 
		kalmanMeasurement, 0, m_state.value.size() );
}