void Camera::lookAt(Vector3f eye, Vector3f center, Vector3f up)
{
Vector3f view = center - eye;
view = view.Normalize();
Vector3f side = view.Cross(up);
side = side.Normalize();
Vector3f u = side.Cross(view);
//the minuses are due to opengl's norm of an inverted z
Matrix4f rotation = Matrix4f({
side.x, u.x, -view.x, 0,
side.y, u.y, -view.y, 0,
side.z, u.z, -view.z, 0,
0, 0, 0, 1
});
Matrix4f translation = MatrixFactory::Translation4(-eye);
this->View = rotation * translation;
CameraChanged = true;
}
void Node::getCameraOrientation(Matrix4f& dst)
{
Vector3f N = _camera.getCameraTarget();
N.Normalize(); //lookAt normalized
Vector3f U = _camera.getCameraUpDirection();
U.Normalize();
U = U.Cross(N);
Vector3f V = N.Cross(U);
V.Normalize();
//Vector3f U = N.Cross(V);
//U.Normalize();
dst.m[0][0] = U.getX();
dst.m[0][1] = U.getY();
dst.m[0][2] = U.getZ();
dst.m[1][0] = V.getX();
dst.m[1][1] = V.getY();
dst.m[1][2] = V.getZ();
dst.m[2][0] = N.getX();
dst.m[2][1] = N.getY();
dst.m[2][2] = N.getZ();
}
//----------------------------------------------------------------------------
void BouncingTetrahedra::DoImpulse (float* impulseMagnitudes)
{
for (int i = 0; i < mNumContacts; ++i)
{
Contact& contact = mContacts[i];
RigidBodyf& bodyA = *contact.A;
RigidBodyf& bodyB = *contact.B;
Vector3f PA = bodyA.GetLinearMomentum();
Vector3f PB = bodyB.GetLinearMomentum();
Vector3f LA = bodyA.GetAngularMomentum();
Vector3f LB = bodyB.GetAngularMomentum();
Vector3f impulse = impulseMagnitudes[i]*contact.N;
PA += impulse;
PB -= impulse;
Vector3f relA = contact.PA - bodyA.GetPosition();
LA += relA.Cross(impulse);
Vector3f relB = contact.PB - bodyB.GetPosition();
LB -= relB.Cross(impulse);
bodyA.SetLinearMomentum(PA);
bodyB.SetLinearMomentum(PB);
bodyA.SetAngularMomentum(LA);
bodyB.SetAngularMomentum(LB);
}
}
Matrix4f Matrix4f::LookAtLH(const Vector3f& eye, const Vector3f& at, const Vector3f& up)
{
Vector3f z = (at - eye).Normalized(); // Forward
Vector3f x = up.Cross(z).Normalized(); // Right
Vector3f y = z.Cross(x);
Matrix4f m(x.x, x.y, x.z, -(x.Dot(eye)),
y.x, y.y, y.z, -(y.Dot(eye)),
z.x, z.y, z.z, -(z.Dot(eye)),
0, 0, 0, 1 );
return m;
}
void Matrix4f::InitCameraTransform(const Vector3f& Target, const Vector3f& Up)
{
Vector3f N = Target;
N.Normalize();
Vector3f U = Up;
U.Normalize();
U = U.Cross(N);
Vector3f V = N.Cross(U);
m[0][0] = U.x; m[0][1] = U.y; m[0][2] = U.z; m[0][3] = 0.0f;
m[1][0] = V.x; m[1][1] = V.y; m[1][2] = V.z; m[1][3] = 0.0f;
m[2][0] = N.x; m[2][1] = N.y; m[2][2] = N.z; m[2][3] = 0.0f;
m[3][0] = 0.0f; m[3][1] = 0.0f; m[3][2] = 0.0f; m[3][3] = 1.0f;
}
bool RtIntersect::RayTriangle( const Vector3f & rayStart, const Vector3f & rayDir,
const Vector3f & v0, const Vector3f & v1, const Vector3f & v2,
float & t0, float & u, float & v )
{
assert( rayDir.IsNormalized() );
const Vector3f edge1 = v1 - v0;
const Vector3f edge2 = v2 - v0;
const Vector3f tv = rayStart - v0;
const Vector3f pv = rayDir.Cross( edge2 );
const Vector3f qv = tv.Cross( edge1 );
const float det = edge1.Dot( pv );
// If the determinant is negative then the triangle is backfacing.
if ( det <= 0.0f )
{
return false;
}
// This code has been modified to only perform a floating-point
// division if the ray actually hits the triangle. If back facing
// triangles are not culled then the sign of 's' and 't' need to
// be flipped. This can be accomplished by multiplying the values
// with the determinant instead of the reciprocal determinant.
const float s = tv.Dot( pv );
const float t = rayDir.Dot( qv );
if ( s >= 0.0f && s <= det )
{
if ( t >= 0.0f && s + t <= det )
{
// If the determinant is almost zero then the ray lies in the triangle plane.
// This comparison is done last because it is usually rare for
// the ray to lay in the triangle plane.
if ( fabsf( det ) > Math<float>::SmallestNonDenormal )
{
const float rcpDet = 1.0f / det;
t0 = edge2.Dot( qv ) * rcpDet;
u = s * rcpDet;
v = t * rcpDet;
return true;
}
}
}
return false;
}
//-----------------------------------------------------------------------
void Camera::_UpdateViewImpl(void)
{
const Vector3f Vaxis(0.0f, 1.0f, 0.0f);
Vector3f View(1.0f, 0.0f, 0.0f);
View.Rotate(m_angleHorizontal, Vaxis);
View.Normalize();
Vector3f Haxis = View.Cross(Vaxis);
Haxis.Normalize();
View.Rotate(m_angleVertical, Haxis);
m_targetVector= View;
m_targetVector.Normalize();
m_upVector = Haxis.Cross(m_targetVector);
m_upVector.Normalize();
Vector3f pos = m_positionVector;
if(mp_parentNode)
{
pos += mp_parentNode->_GetDerivedPosition();
}
m_viewMatrix.InitCameraTransform(pos, m_targetVector, m_upVector);
}
Quaternion Transform::GetLookAtDirection(Vector3f point, Vector3f up)
{
Vector3f lookDir = (point - pos).Normalized();
up = up.Normalized();
// .. What if up and lookDir are parallel?
if (up == lookDir || up == -lookDir)
{
lookDir.y += 0.00000001f;
lookDir.x += 0.00000001f;
}
Vector3f right = up.Cross(lookDir).Normalized();
Vector3f pUp = lookDir.Cross(right).Normalized();
return Quaternion(Matrix4f().InitRotation(lookDir, pUp));
//return Quaternion(Matrix4f().InitRotation(lookDir, up));
}
void Matrix::LookAt(const Vector3f & inEye, const Vector3f & inDirection, const Vector3f & inUp)
{
Vector3f forward = inDirection.Normalized();
Vector3f up = inUp.Normalized();
Vector3f right = forward.Cross(up);
right.Normalize();
up = right.Cross(forward);
// Make inverse rotation matrix using right, forward, up vectors
Set( right.x(), right.y(), right.z(), 0.0f,
up.x(), up.y(), up.z(), 0.0f,
forward.x(), forward.y(), forward.z(), 0.0f,
0.0f, 0.0f, 0.0f, 1.0f);
Pos() = Rotate(-inEye);
}
void Projectile::Update(float elapsedTime)
{
// Test si les conditions de fin du projectile sont vraies
if (m_timeToLive <= 0 || !m_shot || elapsedTime == 0)
return;
// Test si atteint la cible
if( abs(m_destination.x - m_pos.x) < m_collisionRadius.x
&& abs(m_destination.y - m_pos.y) < m_collisionRadius.y
&& abs(m_destination.z - m_pos.z) < m_collisionRadius.z) {
Hit();
return;
}
Vector3f speed = m_rot * m_speed;
speed = speed * m_speed.Lenght();
// Met a jour la valeur de la vitesse en
// fonction de l'acceleration et du temps
m_speed += m_acceleration * elapsedTime;
m_timeToLive -= elapsedTime;
// distance entre le projectile et sa destination
// chemin le plus court
Vector3f distance;
distance.x = m_pos.x - m_destination.x;
distance.y = m_pos.y - m_destination.y;
distance.z = m_pos.z - m_destination.z;
// calculer l'angle entre les 2 vecteurs
// fix imprecision float
float n = distance.Dot(speed) / (distance.Lenght() * speed.Lenght());
if (n > 1)
n = 1;
else if (n < -1)
n = -1;
float angleA = acos(n);
std::cout << angleA << std::endl;
Vector3f axis = distance.Cross(speed);
axis.Normalize();
// rotation autour de laxe
float rotation;
if (abs(angleA) >= m_maxRot && abs(angleA) < PII - m_maxRot) {
rotation = (angleA > 0) ? -m_maxRot : m_maxRot;
rotation *= elapsedTime;
}
else
rotation = angleA - PII;
Quaternion q;
q.FromAxis(rotation, axis);
q.Normalise();
m_rot = q * m_rot;
m_rot.Normalise();
// calcul la nouvelle position
m_pos += speed * elapsedTime;
}
Vector3f MakePosition( float const fwdDist, float const upDist, float const leftDist )
{
static const Vector3f FWD( 0.0f, 0.0f, -1.0f );
static const Vector3f UP( 0.0f, 1.0f, 0.0f );
static const Vector3f LEFT( FWD.Cross( UP ) );
// we only scale left and up by texel scale because those correspond to width / height
return FWD * fwdDist + ( UP * upDist + LEFT * leftDist ) * VRMenuObject::DEFAULT_TEXEL_SCALE;
}
//----------------------------------------------------------------------------
void BouncingTetrahedra::ComputeImpulseMagnitude (float* preRelVelocities,
float* impulseMagnitudes)
{
// The coefficient of restitution.
float restitution = 0.8f;
float temp = 20.0f*NUM_TETRA;
if (mTotalKE < temp)
{
restitution *= 0.5f*mTotalKE/temp;
}
float coeff = -(1.0f + restitution);
for (int i = 0; i < mNumContacts; ++i)
{
if (preRelVelocities[i] < 0.0f)
{
const Contact& contact = mContacts[i];
const RigidBodyf& bodyA = *contact.A;
const RigidBodyf& bodyB = *contact.B;
Vector3f velDiff = bodyA.GetLinearVelocity() -
bodyB.GetLinearVelocity();
Vector3f relA = contact.PA - bodyA.GetPosition();
Vector3f relB = contact.PB - bodyB.GetPosition();
Vector3f AxN = relA.Cross(contact.N);
Vector3f BxN = relB.Cross(contact.N);
Vector3f JInvAxN = bodyA.GetWorldInverseInertia()*AxN;
Vector3f JInvBxN = bodyB.GetWorldInverseInertia()*BxN;
float numer = coeff*(contact.N.Dot(velDiff)
+ bodyA.GetAngularVelocity().Dot(AxN)
- bodyB.GetAngularVelocity().Dot(BxN));
float denom = bodyA.GetInverseMass() + bodyB.GetInverseMass()
+ AxN.Dot(JInvAxN) + BxN.Dot(JInvBxN);
impulseMagnitudes[i] = numer/denom;
}
else
{
impulseMagnitudes[i] = 0.0f;
}
}
}
void Pipeline::InitCameraTransfrom(Matrix4f& CameraTrans,Matrix4f& CameraRot)
{
CameraTrans.mat[0][0] = 1.0f;CameraTrans.mat[0][1] = 0.0f;CameraTrans.mat[0][2] = 0.0f;CameraTrans.mat[0][3] = -m_camera.Pos.x;
CameraTrans.mat[1][0] = 0.0f;CameraTrans.mat[1][1] = 1.0f;CameraTrans.mat[1][2] = 0.0f;CameraTrans.mat[1][3] = -m_camera.Pos.y;
CameraTrans.mat[2][0] = 0.0f;CameraTrans.mat[2][1] = 0.0f;CameraTrans.mat[2][2] = 1.0f;CameraTrans.mat[2][3] = -m_camera.Pos.z;
CameraTrans.mat[3][0] = 0.0f;CameraTrans.mat[3][1] = 0.0f;CameraTrans.mat[3][2] = 0.0f;CameraTrans.mat[3][3] = 1.0f;
Vector3f N = m_camera.Target;
N.Normalize();
Vector3f U = m_camera.Up;
U.Normalize();
U = U.Cross(N);
Vector3f V = N.Cross(U);
CameraRot.mat[0][0] = U.x;CameraRot.mat[0][1] = U.y;CameraRot.mat[0][2] = U.z;CameraRot.mat[0][3] = 0.0f;
CameraRot.mat[1][0] = V.x;CameraRot.mat[1][1] = V.y;CameraRot.mat[1][2] = V.z;CameraRot.mat[1][3] = 0.0f;
CameraRot.mat[2][0] = N.x;CameraRot.mat[2][1] = N.y;CameraRot.mat[2][2] = N.z;CameraRot.mat[2][3] = 0.0f;
CameraRot.mat[3][0] = 0.0f;CameraRot.mat[3][1] = 0.0f;CameraRot.mat[3][2] = 0.0f;CameraRot.mat[3][3] = 1.0f;
}
Matrix4f Matrix4f::Camera(Vector3f target, Vector3f up) {
target.Normalize();
up.Normalize();
up = up.Cross(target);
Vector3f right = target.Cross(up);
Matrix4f m;
m.SetElement(0, 0, up[0]);
m.SetElement(0, 1, up[1]);
m.SetElement(0, 2, up[2]);
m.SetElement(1, 0, right[0]);
m.SetElement(1, 1, right[1]);
m.SetElement(1, 2, right[2]);
m.SetElement(2, 0, target[0]);
m.SetElement(2, 1, target[1]);
m.SetElement(2, 2, target[2]);
m.SetElement(3, 3, 1.0f);
return(m);
}
// Compute a rotation required to transform "estimated" into "measured"
// Returns an approximation of the goal rotation in the Simultaneous Orthogonal Rotations Angle representation
// (vector direction is the axis of rotation, norm is the angle)
Vector3f SensorFusion_ComputeCorrection(Vector3f measured, Vector3f estimated)
{
measured.Normalize();
estimated.Normalize();
Vector3f correction = measured.Cross(estimated);
float cosError = measured.Dot(estimated);
// from the def. of cross product, correction.Length() = sin(error)
// therefore sin(error) * sqrt(2 / (1 + cos(error))) = 2 * sin(error / 2) ~= error in [-pi, pi]
// Mathf::Tolerance is used to avoid div by 0 if cos(error) = -1
return correction * sqrt(2 / (1 + cosError + Mathf::Tolerance));
}
traceResult_t RtTrace::Trace_Exhaustive( const Vector3f & start, const Vector3f & end ) const
{
traceResult_t result;
result.triangleIndex = -1;
result.fraction = 1.0f;
result.uv = Vector2f( 0.0f );
result.normal = Vector3f( 0.0f );
const Vector3f rayDelta = end - start;
const float rayLengthSqr = rayDelta.LengthSq();
const float rayLengthRcp = RcpSqrt( rayLengthSqr );
const float rayLength = rayLengthSqr * rayLengthRcp;
const Vector3f rayStart = start;
const Vector3f rayDir = rayDelta * rayLengthRcp;
float bestDistance = rayLength;
Vector2f uv;
for ( int i = 0; i < header.numIndices; i += 3 )
{
float distance;
float u;
float v;
if ( RtIntersect::RayTriangle( rayStart, rayDir,
vertices[indices[i + 0]],
vertices[indices[i + 1]],
vertices[indices[i + 2]], distance, u, v ) )
{
if ( distance >= 0.0f && distance < bestDistance )
{
bestDistance = distance;
result.triangleIndex = i;
uv.x = u;
uv.y = v;
}
}
}
if ( result.triangleIndex != -1 )
{
result.fraction = bestDistance * rayLengthRcp;
result.uv = uvs[indices[result.triangleIndex + 0]] * ( 1.0f - uv.x - uv.y ) +
uvs[indices[result.triangleIndex + 1]] * uv.x +
uvs[indices[result.triangleIndex + 2]] * uv.y;
const Vector3f d1 = vertices[indices[result.triangleIndex + 1]] - vertices[indices[result.triangleIndex + 0]];
const Vector3f d2 = vertices[indices[result.triangleIndex + 2]] - vertices[indices[result.triangleIndex + 0]];
result.normal = d1.Cross( d2 ).Normalized();
}
return result;
}
//----------------------------------------------------------------------------
void BouncingTetrahedra::ComputePreimpulseVelocity (float* preRelVelocities)
{
for (int i = 0; i < mNumContacts; ++i)
{
const Contact& contact = mContacts[i];
const RigidBodyf& bodyA = *contact.A;
const RigidBodyf& bodyB = *contact.B;
Vector3f XA = bodyA.GetPosition();
Vector3f XB = bodyB.GetPosition();
Vector3f VA = bodyA.GetLinearVelocity();
Vector3f VB = bodyB.GetLinearVelocity();
Vector3f WA = bodyA.GetAngularVelocity();
Vector3f WB = bodyB.GetAngularVelocity();
Vector3f relA = contact.PA - XA;
Vector3f relB = contact.PB - XB;
Vector3f velA = VA + WA.Cross(relA);
Vector3f velB = VB + WB.Cross(relB);
preRelVelocities[i] = contact.N.Dot(velB - velA);
}
}
const Vector3f Triangle::GetNormal() const
{
Vector3f normal;
Vector3f a = Point2 - Point1;
Vector3f b = Point3 - Point1;
normal = a.Cross(b);
normal.Normalize();
return normal;
}
void Camera::Move(float x, float y, float z) {
// posunout o z-nasobek smeru pohledu
Vector3f dir = target;
dir.Normalize();
eye += dir * z;
// posunout do strany o x-nasobek right vektoru
Vector3f r = target;
r.Normalize();
r = -r.Cross(up);
eye += r * x;
}
void CRigidBody::HandleSphereToSphereCollision(CCollisionMessage* ColMsg)
{
CVehicle* Car = (CVehicle*)ColMsg->GetEntity();
Vector3f CenterToCenter = *ColMsg->GetCenterToCenter();
// set translate
m_vReflection = CenterToCenter*0.05f;
m_vPosition = m_translate;
m_vDirectionWhenDisturbed = m_vReflection;
// set rotation
if (CenterToCenter.Length() == 0.0f) CenterToCenter = Vector3f(1.0f, 0.0f, 0.0f);
float theta1 = (CenterToCenter.Dot(Car->GetVehicleHeadingWC()))/(CenterToCenter.Length()*Car->GetVehicleHeadingWC().Length());
Vector3f CP = CenterToCenter.Cross(Vector3f(0.0f, 1.0f, 0.0f));
float theta2 = (CP.Dot(Car->GetVehicleHeadingWC()))/(CP.Length()*Car->GetVehicleHeadingWC().Length());
// CLog::GetLog().Write(LOG_DEBUGOVERLAY, 120, "theta1 = %f", theta1);
// CLog::GetLog().Write(LOG_DEBUGOVERLAY, 121, "theta2 = %f", theta2);
float spin;
if (0.0f < theta1 && theta1 < 1.0f) {
// LOWER-LEFT QUADRANT
if (0.0f < theta2 && theta2 < 1.0f) {
spin = theta2;
}
// LOWER-RIGHT QUADRANT
else {
spin = theta2;
}
}
else {
// UPPER-LEFT QUADRANT
if (0.0f < theta2 && theta2 < 1.0f) {
spin = -theta2;
}
// UPPER-RIGHT QUADRANT
else {
spin = -theta2;
}
}
spin *= RIGIDBODY_SPIN_FACTOR;
m_vRotation = Vector3f(0.0f, spin, 0.0f);
// actually set it!
Car->SetVehicleVelocityLC(0.0f);
Car->SetVehiclePositionLC(Vector3f(m_vPosition.X(), m_vPosition.Z(), m_vPosition.Y()));
disturbed = true;
}
void Camera::quaternionLookAt(float rotationX, float rotationY, float zoom, Vector3f eye, Vector3f center, Vector3f up)
{
Vector3f view = Vector3f(0, 0, 1);//center - eye;
view = view.Normalize();
Vector3f side = view.Cross(up);
side = side.Normalize();
Vector3f u = side.Cross(view);
Quaternion verticalRotation = Quaternion::FromAngleAxis(rotationY, side);
Quaternion horizontalRotation = Quaternion::FromAngleAxis(-rotationX, u);
Quaternion rotation = verticalRotation * horizontalRotation;
Matrix4f translation = MatrixFactory::Translation4(Vector3f(center.x, center.y, -(zoom + eye.z)));
this->View = translation * rotation.ToMatrix4();
CameraChanged = true;
}
//----------------------------------------------------------------------------------------------------
Matrix4f Matrix4f::LookAtLHMatrix( Vector3f& eye, Vector3f& at, Vector3f& up )
{
// This method is based on the method of the same name from the D3DX library.
Matrix4f ret;
Vector3f zaxis = at - eye;
zaxis.Normalize();
Vector3f xaxis = up.Cross( zaxis );
xaxis.Normalize();
Vector3f yaxis = zaxis.Cross( xaxis );
ret.m_afEntry[ 0] = xaxis.x;
ret.m_afEntry[ 1] = yaxis.x;
ret.m_afEntry[ 2] = zaxis.x;
ret.m_afEntry[ 3] = 0.0f;
ret.m_afEntry[ 4] = xaxis.y;
ret.m_afEntry[ 5] = yaxis.y;
ret.m_afEntry[ 6] = zaxis.y;
ret.m_afEntry[ 7] = 0.0f;
ret.m_afEntry[ 8] = xaxis.z;
ret.m_afEntry[ 9] = yaxis.z;
ret.m_afEntry[10] = zaxis.z;
ret.m_afEntry[11] = 0.0f;
ret.m_afEntry[12] = -(xaxis.Dot(eye));
ret.m_afEntry[13] = -(yaxis.Dot(eye));
ret.m_afEntry[14] = -(zaxis.Dot(eye));
ret.m_afEntry[15] = 1.0f;
return( ret );
}
void Camera::rodriguesLookAt(float rotationX, float rotationY, Vector3f eye, Vector3f center, Vector3f up)
{
Vector3f view = Vector3f(0, 0, 1);//center - eye;
view = view.Normalize();
Vector3f side = view.Cross(up);
side = side.Normalize();
Vector3f u = side.Cross(view);
Matrix4f rotationMatX = MatrixFactory::Rotation4(rotationY, side);
Matrix4f rotationMatY = MatrixFactory::Rotation4(-rotationX, u);
Matrix4f rotation = rotationMatX * rotationMatY;
Matrix4f translation = MatrixFactory::Translation4(Vector3f(center.x, center.y, eye.z));
this->View = translation * rotation;
CameraChanged = true;
}
void Camera::Update()
{
const Vector3f Vaxis(0.0f, 1.0f, 0.0f);
Vector3f View(1.0f, 0.0f, 0.0f);
View.Rotate(m_AngleH, Vaxis);
View.Normalize();
Vector3f Haxis = Vaxis.Cross(View);
Haxis.Normalize();
View.Rotate(m_AngleV, Haxis);
m_target = View;
m_target.Normalize();
m_up = m_target.Cross(Haxis);
m_up.Normalize();
}
//-----------------------------------------------------------------------
void Frustum::_UpdateViewImpl(void)
{
const Vector3f Vaxis(0.0f, 1.0f, 0.0f);
Vector3f View(1.0f, 0.0f, 0.0f);
View.Rotate(m_angleHorizontal, Vaxis);
View.Normalize();
Vector3f Haxis = View.Cross(Vaxis);
Haxis.Normalize();
View.Rotate(m_angleVertical, Haxis);
m_targetVector= View;
m_targetVector.Normalize();
m_upVector = Haxis.Cross(m_targetVector);
m_upVector.Normalize();
m_viewMatrix.InitCameraTransform(m_positionVector,m_targetVector,m_upVector);
}
void Camera::Update()
{
const Vector3f Vaxis(0.0f, 1.0f, 0.0f);
// Rotate the view vector by the horizontal angle around the vertical axis
Vector3f View(1.0f, 0.0f, 0.0f);
View.Rotate(m_AngleH, Vaxis);
View.Normalize();
// Rotate the view vector by the vertical angle around the horizontal axis
Vector3f Haxis = Vaxis.Cross(View);
Haxis.Normalize();
View.Rotate(m_AngleV, Haxis);
m_target = View;
m_target.Normalize();
m_up = m_target.Cross(Haxis);
m_up.Normalize();
}
/* Calculates the GLOBAL rotation of the world, not relative to the world's current rotation
* * pFrom, the first point,
* * pDest, the next point. The two points create a line and the line's angle from the x axis is measured */
double SplineTraveler::calcRotation(Vector3f axis, Vector3f pVertex, Vector3f pDest)
{
//Vector3f firstBranch = (pFrom - pVertex).Normalize();
Vector3f secondBranch = (pDest - pVertex).Normalize();
//TODO handle 180 degree case
float railAngle;
//dot product angle. magtides of the vectors are already equal to one
railAngle = acos(axis.Dot(secondBranch));
if(secondBranch.z > 0)
{
railAngle = 2.0*3.14159265 - railAngle;
}
rotationAngle = railAngle;
rotateAxis = secondBranch.Cross(axis); //axis of this rotation.
//rotateAxis = Vector3f(0,1,0);
/*Vector3f xaxis = Vector3f(1,0,0);
Vector3f yaxis = Vector3f(0,1,0);
Vector3f zaxis = Vector3f(0,0,1);
Vector3f point = pDest - pFrom;
point.y = 0;
bool tr = false;
//if(!(point.x == 0 && point.y == 0))
{
tr = true;
double x = acos((point.Dot(xaxis))/(point.Magnitude()));
if(point.z > 0)
{
x = 2.0*3.14159 - x;
}
//double y = acos((point.Dot(yaxis))/(point.Magnitude()));
double y = 0;
double z = 0;//acos((point.Dot(zaxis))/(point.Magnitude()));
grotation = Vector3f(x,y,z);
}*/
return railAngle;
}
//==============================
// BitmapFontSurfaceLocal::DrawText3D
void BitmapFontSurfaceLocal::DrawText3D( BitmapFont const & font, fontParms_t const & parms,
Vector3f const & pos, Vector3f const & normal, Vector3f const & up,
float scale, Vector4f const & color, char const * text )
{
if ( text == NULL || text[0] == '\0' )
{
return; // nothing to do here, move along
}
// TODO: multiple line support -- we would need to calculate the horizontal width
// for each string ending in \n
size_t len;
float width;
float height;
float ascent;
float descent;
int const MAX_LINES = 128;
float lineWidths[MAX_LINES];
int numLines;
AsLocal( font ).CalcTextMetrics( text, len, width, height, ascent, descent, lineWidths, MAX_LINES, numLines );
// LOG( "BitmapFontSurfaceLocal::DrawText3D( \"%s\" %s %s ) : width = %.2f, height = %.2f, numLines = %i, fh = %.2f",
// text, parms.CenterVert ? "cv" : "", parms.CenterHoriz ? "ch" : "",
// width, height, numLines, AsLocal( font ).GetFontInfo().FontHeight );
if ( len == 0 )
{
return;
}
DROID_ASSERT( normal.IsNormalized(), "BitmapFont" );
DROID_ASSERT( up.IsNormalized(), "BitmapFont" );
const FontInfoType & fontInfo = AsLocal( font ).GetFontInfo();
float imageWidth = (float)AsLocal( font ).GetImageWidth();
float const xScale = AsLocal( font ).GetFontInfo().ScaleFactor * scale;
float const yScale = AsLocal( font ).GetFontInfo().ScaleFactor * scale;
// allocate a vertex block
size_t numVerts = 4 * len;
VertexBlockType vb( font, numVerts, pos, Quatf(), parms.Billboard, parms.TrackRoll );
Vector3f const right = up.Cross( normal );
Vector3f const r = ( parms.Billboard ) ? Vector3f( 1.0f, 0.0f, 0.0f ) : right;
Vector3f const u = ( parms.Billboard ) ? Vector3f( 0.0f, 1.0f, 0.0f ) : up;
Vector3f curPos( 0.0f );
if ( parms.CenterVert )
{
float const vofs = ( height * 0.5f ) - ascent;
curPos += u * ( vofs * scale );
}
Vector3f basePos = curPos;
if ( parms.CenterHoriz )
{
curPos -= r * ( lineWidths[0] * 0.5f * scale );
}
Vector3f lineInc = u * ( fontInfo.FontHeight * yScale );
float const distanceScale = imageWidth / FontInfoType::DEFAULT_SCALE_FACTOR;
const uint8_t fontParms[4] =
{
(uint8_t)( OVR::Alg::Clamp( parms.AlphaCenter + fontInfo.CenterOffset, 0.0f, 1.0f ) * 255 ),
(uint8_t)( OVR::Alg::Clamp( parms.ColorCenter + fontInfo.CenterOffset, 0.0f, 1.0f ) * 255 ),
(uint8_t)( OVR::Alg::Clamp( distanceScale, 1.0f, 255.0f ) ),
0
};
int iColor = ColorToABGR( color );
int curLine = 0;
fontVertex_t * v = vb.Verts;
char const * p = text;
size_t i = 0;
uint32_t charCode = UTF8Util::DecodeNextChar( &p );
for ( ; charCode != '\0'; i++, charCode = UTF8Util::DecodeNextChar( &p ) )
{
OVR_ASSERT( i < len );
if ( charCode == '\n' && curLine < numLines && curLine < MAX_LINES )
{
// move to next line
curLine++;
basePos -= lineInc;
curPos = basePos;
if ( parms.CenterHoriz )
{
curPos -= r * ( lineWidths[curLine] * 0.5f * scale );
}
}
FontGlyphType const & g = AsLocal( font ).GlyphForCharCode( charCode );
float s0 = g.X;
float t0 = g.Y;
float s1 = ( g.X + g.Width );
float t1 = ( g.Y + g.Height );
float bearingX = g.BearingX * xScale;
float bearingY = g.BearingY * yScale ;
float rw = ( g.Width + g.BearingX ) * xScale;
float rh = ( g.Height - g.BearingY ) * yScale;
// lower left
v[i * 4 + 0].xyz = curPos + ( r * bearingX ) - ( u * rh );
v[i * 4 + 0].s = s0;
v[i * 4 + 0].t = t1;
*(UInt32*)(&v[i * 4 + 0].rgba[0]) = iColor;
*(UInt32*)(&v[i * 4 + 0].fontParms[0]) = *(UInt32*)(&fontParms[0]);
// upper left
v[i * 4 + 1].xyz = curPos + ( r * bearingX ) + ( u * bearingY );
v[i * 4 + 1].s = s0;
v[i * 4 + 1].t = t0;
*(UInt32*)(&v[i * 4 + 1].rgba[0]) = iColor;
*(UInt32*)(&v[i * 4 + 1].fontParms[0]) = *(UInt32*)(&fontParms[0]);
// upper right
v[i * 4 + 2].xyz = curPos + ( r * rw ) + ( u * bearingY );
v[i * 4 + 2].s = s1;
v[i * 4 + 2].t = t0;
*(UInt32*)(&v[i * 4 + 2].rgba[0]) = iColor;
*(UInt32*)(&v[i * 4 + 2].fontParms[0]) = *(UInt32*)(&fontParms[0]);
// lower right
v[i * 4 + 3].xyz = curPos + ( r * rw ) - ( u * rh );
v[i * 4 + 3].s = s1;
v[i * 4 + 3].t = t1;
*(UInt32*)(&v[i * 4 + 3].rgba[0]) = iColor;
*(UInt32*)(&v[i * 4 + 3].fontParms[0]) = *(UInt32*)(&fontParms[0]);
// advance to start of next char
curPos += r * ( g.AdvanceX * xScale );
}
// add the new vertex block to the array of vertex blocks
VertexBlocks.PushBack( vb );
}
//---------------------------------------------------------------
// Purpose: raydir must be normalized!
//---------------------------------------------------------------
float RayPointDistance( const Vector3f &rayorig, const Vector3f &raydir, const Vector3f &point )
{
return raydir.Cross(point - rayorig).Length();
}
//----------------------------------------------------------------------------------------------------
Vector3f Vector3f::Cross( const Vector3f& A, const Vector3f& B )
{
return A.Cross( B );
}