bullet3

#include "SimpleCamera.h"

#include "Bullet3Common/b3Vector3.h"
#include "Bullet3Common/b3Quaternion.h"
#include "Bullet3Common/b3Matrix3x3.h"
#include "Bullet3Common/b3Transform.h"

B3_ATTRIBUTE_ALIGNED16(struct)
SimpleCameraInternalData
{
	SimpleCameraInternalData()
		: m_cameraTargetPosition(b3MakeVector3(0, 0, 0)),
		  m_cameraDistance(20),
		  m_cameraUp(b3MakeVector3(0, 1, 0)),
		  m_cameraForward(b3MakeVector3(1, 0, 0)),
		  m_cameraUpAxis(1),
		  m_yaw(20),
		  m_pitch(0),
		  m_aspect(1),
		  m_frustumZNear(0.01),
		  m_frustumZFar(1000),
		  m_enableVR(false)
	{
		b3Transform tr;
		tr.setIdentity();
		tr.getOpenGLMatrix(m_offsetTransformVR);
	}

	B3_DECLARE_ALIGNED_ALLOCATOR();

	B3_ATTRIBUTE_ALIGNED16(float)
	m_offsetTransformVR[16];
	b3Vector3 m_cameraTargetPosition;
	float m_cameraDistance;
	b3Vector3 m_cameraUp;
	b3Vector3 m_cameraForward;
	int m_cameraUpAxis;
	//the m_cameraPosition is a cached value, recomputed from other values
	b3Vector3 m_cameraPosition;
	float m_yaw;

	float m_pitch;
	float m_aspect;
	float m_frustumZNear;
	float m_frustumZFar;

	bool m_enableVR;
	float m_viewMatrixVR[16];
	float m_projectionMatrixVR[16];
};

SimpleCamera::SimpleCamera()
{
	m_data = new SimpleCameraInternalData;
}
SimpleCamera::~SimpleCamera()
{
	delete m_data;
}

void SimpleCamera::setVRCamera(const float viewMat[16], const float projectionMatrix[16])
{
	m_data->m_enableVR = true;

	b3Matrix3x3 vm;
	vm.setValue(viewMat[0], viewMat[4], viewMat[8],
				viewMat[1], viewMat[5], viewMat[9],
				viewMat[2], viewMat[6], viewMat[10]);

	b3Vector3 vp = b3MakeVector3(viewMat[12], viewMat[13], viewMat[14]);
	b3Transform tr;
	tr.setBasis(vm);
	tr.setOrigin(vp);
	b3Transform cp = tr.inverse();
	m_data->m_cameraPosition = cp.getOrigin();

	for (int i = 0; i < 16; i++)
	{
		m_data->m_viewMatrixVR[i] = viewMat[i];
		m_data->m_projectionMatrixVR[i] = projectionMatrix[i];
		m_data->m_frustumZNear = m_data->m_projectionMatrixVR[14] / (m_data->m_projectionMatrixVR[10] - 1);
		m_data->m_frustumZFar = m_data->m_projectionMatrixVR[14] / (m_data->m_projectionMatrixVR[10] + 1);
	}
}

bool SimpleCamera::getVRCamera(float viewMat[16], float projectionMatrix[16])
{
	if (m_data->m_enableVR)
	{
		for (int i = 0; i < 16; i++)
		{
			viewMat[i] = m_data->m_viewMatrixVR[i];
			projectionMatrix[i] = m_data->m_projectionMatrixVR[i];
		}
	}
	return false;
}

void SimpleCamera::disableVRCamera()
{
	m_data->m_enableVR = false;
}

bool SimpleCamera::isVRCamera() const
{
	return m_data->m_enableVR;
}

static void b3CreateFrustum(
	float left,
	float right,
	float bottom,
	float top,
	float nearVal,
	float farVal,
	float frustum[16])
{
	frustum[0 * 4 + 0] = (float(2) * nearVal) / (right - left);
	frustum[0 * 4 + 1] = float(0);
	frustum[0 * 4 + 2] = float(0);
	frustum[0 * 4 + 3] = float(0);

	frustum[1 * 4 + 0] = float(0);
	frustum[1 * 4 + 1] = (float(2) * nearVal) / (top - bottom);
	frustum[1 * 4 + 2] = float(0);
	frustum[1 * 4 + 3] = float(0);

	frustum[2 * 4 + 0] = (right + left) / (right - left);
	frustum[2 * 4 + 1] = (top + bottom) / (top - bottom);
	frustum[2 * 4 + 2] = -(farVal + nearVal) / (farVal - nearVal);
	frustum[2 * 4 + 3] = float(-1);

	frustum[3 * 4 + 0] = float(0);
	frustum[3 * 4 + 1] = float(0);
	frustum[3 * 4 + 2] = -(float(2) * farVal * nearVal) / (farVal - nearVal);
	frustum[3 * 4 + 3] = float(0);
}

#if 0
static void b3CreateDiagonalMatrix(float value, float result[4][4])
{
	for (int i=0;i<4;i++)
	{
		for (int j=0;j<4;j++)
		{
			if (i==j)
			{
				result[i][j] = value;
			} else
			{
				result[i][j] = 0.f;
			}
		}
	}
}
static void b3CreateOrtho(float left, float right, float bottom, float top, float zNear, float zFar, float result[4][4])
{
	b3CreateDiagonalMatrix(1.f,result);

	result[0][0] = 2.f / (right - left);
	result[1][1] = 2.f / (top - bottom);
	result[2][2] = - 2.f / (zFar - zNear);
	result[3][0] = - (right + left) / (right - left);
	result[3][1] = - (top + bottom) / (top - bottom);
	result[3][2] = - (zFar + zNear) / (zFar - zNear);
}
#endif
static void b3CreateLookAt(const b3Vector3& eye, const b3Vector3& center, const b3Vector3& up, float result[16])
{
	b3Vector3 f = (center - eye).normalized();
	b3Vector3 u = up.normalized();
	b3Vector3 s = (f.cross(u)).normalized();
	u = s.cross(f);

	result[0 * 4 + 0] = s.x;
	result[1 * 4 + 0] = s.y;
	result[2 * 4 + 0] = s.z;

	result[0 * 4 + 1] = u.x;
	result[1 * 4 + 1] = u.y;
	result[2 * 4 + 1] = u.z;

	result[0 * 4 + 2] = -f.x;
	result[1 * 4 + 2] = -f.y;
	result[2 * 4 + 2] = -f.z;

	result[0 * 4 + 3] = 0.f;
	result[1 * 4 + 3] = 0.f;
	result[2 * 4 + 3] = 0.f;

	result[3 * 4 + 0] = -s.dot(eye);
	result[3 * 4 + 1] = -u.dot(eye);
	result[3 * 4 + 2] = f.dot(eye);
	result[3 * 4 + 3] = 1.f;
}

void SimpleCamera::setCameraUpAxis(int upAxis)
{
	m_data->m_cameraUpAxis = upAxis;

	update();
}

int SimpleCamera::getCameraUpAxis() const
{
	return m_data->m_cameraUpAxis;
}

void SimpleCamera::update()
{
	b3Scalar yawRad = m_data->m_yaw * b3Scalar(0.01745329251994329547);      // rads per deg
	b3Scalar pitchRad = m_data->m_pitch * b3Scalar(0.01745329251994329547);  // rads per deg
	b3Scalar rollRad = 0.0;
	b3Quaternion eyeRot;

	int forwardAxis(-1);
	switch (m_data->m_cameraUpAxis)
	{
		case 1:
			forwardAxis = 2;
			m_data->m_cameraUp = b3MakeVector3(0, 1, 0);
			//gLightPos = b3MakeVector3(-50.f,100,30);
			eyeRot.setEulerZYX(rollRad, yawRad, -pitchRad);
			break;
		case 2:
			forwardAxis = 1;
			m_data->m_cameraUp = b3MakeVector3(0, 0, 1);
			//gLightPos = b3MakeVector3(-50.f,30,100);
			eyeRot.setEulerZYX(yawRad, rollRad, pitchRad);
			break;
		default:
		{
			//b3Assert(0);
			return;
		}
	};

	b3Vector3 eyePos = b3MakeVector3(0, 0, 0);
	eyePos[forwardAxis] = -m_data->m_cameraDistance;
	eyePos = b3Matrix3x3(eyeRot) * eyePos;

	m_data->m_cameraPosition = eyePos;

	m_data->m_cameraPosition += m_data->m_cameraTargetPosition;

	m_data->m_cameraForward = m_data->m_cameraTargetPosition - m_data->m_cameraPosition;
	if (m_data->m_cameraForward.length2() < B3_EPSILON)
	{
		m_data->m_cameraForward.setValue(1.f, 0.f, 0.f);
	}
	else
	{
		m_data->m_cameraForward.normalize();
	}
}

void SimpleCamera::getCameraProjectionMatrix(float projectionMatrix[16]) const
{
	if (m_data->m_enableVR)
	{
		for (int i = 0; i < 16; i++)
		{
			projectionMatrix[i] = m_data->m_projectionMatrixVR[i];
		}
	}
	else
	{
		b3CreateFrustum(-m_data->m_aspect * m_data->m_frustumZNear, m_data->m_aspect * m_data->m_frustumZNear, -m_data->m_frustumZNear, m_data->m_frustumZNear, m_data->m_frustumZNear, m_data->m_frustumZFar, projectionMatrix);
	}
}
void SimpleCamera::setVRCameraOffsetTransform(const float offset[16])
{
	for (int i = 0; i < 16; i++)
	{
		m_data->m_offsetTransformVR[i] = offset[i];
	}
}
void SimpleCamera::getCameraViewMatrix(float viewMatrix[16]) const
{
	if (m_data->m_enableVR)
	{
		for (int i = 0; i < 16; i++)
		{
			b3Transform tr;
			tr.setFromOpenGLMatrix(m_data->m_viewMatrixVR);
			b3Transform shift = b3Transform::getIdentity();
			shift.setFromOpenGLMatrix(m_data->m_offsetTransformVR);
			tr = tr * shift;
			tr.getOpenGLMatrix(viewMatrix);
			//viewMatrix[i] = m_data->m_viewMatrixVR[i];
		}
	}
	else
	{
		b3CreateLookAt(m_data->m_cameraPosition, m_data->m_cameraTargetPosition, m_data->m_cameraUp, viewMatrix);
	}
}

void SimpleCamera::getCameraTargetPosition(double pos[3]) const
{
	pos[0] = m_data->m_cameraTargetPosition[0];
	pos[1] = m_data->m_cameraTargetPosition[1];
	pos[2] = m_data->m_cameraTargetPosition[2];
}

void SimpleCamera::getCameraPosition(double pos[3]) const
{
	pos[0] = m_data->m_cameraPosition[0];
	pos[1] = m_data->m_cameraPosition[1];
	pos[2] = m_data->m_cameraPosition[2];
}

void SimpleCamera::getCameraTargetPosition(float pos[3]) const
{
	pos[0] = m_data->m_cameraTargetPosition[0];
	pos[1] = m_data->m_cameraTargetPosition[1];
	pos[2] = m_data->m_cameraTargetPosition[2];
}
void SimpleCamera::getCameraPosition(float pos[3]) const
{
	pos[0] = m_data->m_cameraPosition[0];
	pos[1] = m_data->m_cameraPosition[1];
	pos[2] = m_data->m_cameraPosition[2];
}

void SimpleCamera::setCameraTargetPosition(float x, float y, float z)
{
	m_data->m_cameraTargetPosition.setValue(x, y, z);
	update();
}
float SimpleCamera::getCameraDistance() const
{
	return m_data->m_cameraDistance;
}

void SimpleCamera::setCameraDistance(float dist)
{
	m_data->m_cameraDistance = dist;
	update();
}
void SimpleCamera::setCameraUpVector(float x, float y, float z)
{
	m_data->m_cameraUp.setValue(x, y, z);
	update();
}

void SimpleCamera::getCameraUpVector(float up[3]) const
{
	if (m_data->m_enableVR)
	{
		float viewMatTotal[16];
		getCameraViewMatrix(viewMatTotal);
		up[0] = viewMatTotal[0];
		up[1] = viewMatTotal[4];
		up[2] = viewMatTotal[8];
	}
	else
	{
		up[0] = float(m_data->m_cameraUp[0]);
		up[1] = float(m_data->m_cameraUp[1]);
		up[2] = float(m_data->m_cameraUp[2]);
	}
}

void SimpleCamera::getCameraForwardVector(float fwd[3]) const
{
	if (m_data->m_enableVR)
	{
		float viewMatTotal[16];
		getCameraViewMatrix(viewMatTotal);
		fwd[0] = viewMatTotal[2];
		fwd[1] = viewMatTotal[6];
		fwd[2] = viewMatTotal[10];
	}
	else
	{
		fwd[0] = float(m_data->m_cameraForward[0]);
		fwd[1] = float(m_data->m_cameraForward[1]);
		fwd[2] = float(m_data->m_cameraForward[2]);
	}
}

void SimpleCamera::setCameraYaw(float yaw)
{
	m_data->m_yaw = yaw;
	update();
}

float SimpleCamera::getCameraYaw() const
{
	return m_data->m_yaw;
}

void SimpleCamera::setCameraPitch(float pitch)
{
	m_data->m_pitch = pitch;
	update();
}

void SimpleCamera::setAspectRatio(float ratio)
{
	m_data->m_aspect = ratio;
	update();
}

float SimpleCamera::getCameraPitch() const
{
	return m_data->m_pitch;
}
float SimpleCamera::getAspectRatio() const
{
	return m_data->m_aspect;
}

float SimpleCamera::getCameraFrustumFar() const
{
	return m_data->m_frustumZFar;
}

float SimpleCamera::getCameraFrustumNear() const
{
	return m_data->m_frustumZNear;
}

void SimpleCamera::setCameraFrustumFar(float far)
{
	m_data->m_frustumZFar = far;
}

void SimpleCamera::setCameraFrustumNear(float near)
{
	m_data->m_frustumZNear = near;
}