// Called when the user moves the mouse in the main window
// while a button is held down
void Cmesh_mesh_collisionsApp::scroll(CPoint p, int left_button) {
// If the user hasn't clicked on any objects, we don't
// have to move or rotate anyone
if (selected_object == 0) return;
cGenericObject* object_to_move = selected_object;
// If the left button is being held down, rotate the
// selected object
if (left_button) {
cVector3d axis1(-1,0,0);
object_to_move->rotate(axis1,-1.0*(float)p.y / 50.0);
cVector3d axis2(0,1,0);
object_to_move->rotate(axis2,(float)p.x / 50.0);
}
// If the left button is being held down, move the
// selected object
else {
object_to_move->translate((float)p.x / 100.0, 0, 0);
object_to_move->translate(0, -1.0*(float)p.y / 100.0, 0);
}
// Let the object re-compute his global position data
object->computeGlobalPositions();
object->computeBoundaryBox(true);
}
bool IfcGeom::convert(const Ifc2x3::IfcCartesianTransformationOperator3DnonUniform::ptr l, gp_GTrsf& gtrsf) {
IN_CACHE(IfcCartesianTransformationOperator3DnonUniform,l,gp_GTrsf,gtrsf)
gp_Trsf trsf;
gp_Pnt origin;
IfcGeom::convert(l->LocalOrigin(),origin);
gp_Dir axis1 (1.,0.,0.);
gp_Dir axis2 (0.,1.,0.);
gp_Dir axis3;
if ( l->hasAxis1() ) IfcGeom::convert(l->Axis1(),axis1);
if ( l->hasAxis2() ) IfcGeom::convert(l->Axis2(),axis2);
if ( l->hasAxis3() ) IfcGeom::convert(l->Axis3(),axis3);
else axis3 = axis1.Crossed(axis2);
gp_Ax3 ax3 (origin,axis3,axis1);
if ( axis2.Dot(ax3.YDirection()) < 0 ) ax3.YReverse();
trsf.SetTransformation(ax3);
trsf.Invert();
const double scale1 = l->hasScale() ? l->Scale() : 1.0f;
const double scale2 = l->hasScale2() ? l->Scale2() : scale1;
const double scale3 = l->hasScale3() ? l->Scale3() : scale1;
gtrsf = gp_GTrsf();
gtrsf.SetValue(1,1,scale1);
gtrsf.SetValue(2,2,scale2);
gtrsf.SetValue(3,3,scale3);
gtrsf.PreMultiply(trsf);
CACHE(IfcCartesianTransformationOperator3DnonUniform,l,gtrsf)
return true;
}
SbMatrix tgf::MatrixFromTransform( const Transform& transform )
{
Ptr<Matrix4x4> transformMatrix = transform.GetMatrix()->Transpose();
float m00 = float ( transformMatrix->m[0][0] );
float m01 = float ( transformMatrix->m[0][1] );
float m02 = float ( transformMatrix->m[0][2] );
float m03 = float ( transformMatrix->m[0][3] );
float m10 = float ( transformMatrix->m[1][0] );
float m11 = float ( transformMatrix->m[1][1] );
float m12 = float ( transformMatrix->m[1][2] );
float m13 = float ( transformMatrix->m[1][3] );
float m20 = float ( transformMatrix->m[2][0] );
float m21 = float ( transformMatrix->m[2][1] );
float m22 = float ( transformMatrix->m[2][2] );
float m23 = float ( transformMatrix->m[2][3] );
float m30 = float ( transformMatrix->m[3][0] );
float m31 = float ( transformMatrix->m[3][1] );
float m32 = float ( transformMatrix->m[3][2] );
float m33 = float ( transformMatrix->m[3][3] );
SbVec3f axis1( m00, m10, m20 );
SbVec3f axis2( m01, m11, m21 );
//axis2.normalize();
SbVec3f axis3( m02, m12, m22 );
//axis3.normalize();
return SbMatrix( axis1[0], axis2[0], axis3[0], m03,
axis1[1], axis2[1], axis3[1], m13,
axis1[2], axis2[2], axis3[2], m23,
m30, m31, m32, m33 );
}
bool IfcGeom::convert(const Ifc2x3::IfcCartesianTransformationOperator2D::ptr l, gp_Trsf2d& trsf) {
IN_CACHE(IfcCartesianTransformationOperator2D,l,gp_Trsf2d,trsf)
gp_Pnt origin;
IfcGeom::convert(l->LocalOrigin(),origin);
gp_Dir axis1 (1.,0.,0.);
if ( l->hasAxis1() ) IfcGeom::convert(l->Axis1(),axis1);
const gp_Ax2d ax2d (gp_Pnt2d(origin.X(),origin.Y()),gp_Dir2d(axis1.X(),axis1.Y()));
trsf.SetTransformation(ax2d);
trsf.Invert();
if ( l->hasScale() ) trsf.SetScaleFactor(l->Scale());
CACHE(IfcCartesianTransformationOperator2D,l,trsf)
return true;
}
// Called when the user moves the mouse in the main window
// while a button is held down
void Crecord_playerApp::scroll(CPoint p, int left_button) {
// For now, we're going to disable mouse interaction in
// this example to simplify the interaction between
// the haptic device and the record player...
return;
// If the user hasn't clicked on any objects, we don't
// have to move or rotate anyone
//if (selected_object == 0) return;
cGenericObject* object_to_move = selected_object;
// If the left button is being held down, rotate the
// selected object
if (left_button) {
m_recordMesh->translate(cMul(-0.04, m_recordMesh->getRot().getCol2()));
cVector3d axis1(-1,0,0);
object->rotate(axis1,-1.0*(float)p.y / 50.0);
m_recordMesh->rotate(axis1,-1.0*(float)p.y / 50.0);
cVector3d axis2(0,1,0);
object->rotate(axis2,(float)p.x / 50.0);
m_recordMesh->rotate(axis2,(float)p.x / 50.0);
m_recordMesh->translate(cMul(0.04, m_recordMesh->getRot().getCol2()));
}
// If the left button is being held down, move the
// selected object
else {
object->translate(0, 0, -1.0*(float)p.y / 100.0);
m_recordMesh->translate(0, 0, -1.0*(float)p.y / 100.0);
object->translate(0, 1.0*(float)p.x / 100.0, 0);
m_recordMesh->translate(0, 1.0*(float)p.x / 100.0, 0);
}
// Let the object re-compute his global position data
object->computeGlobalPositions();
m_recordMesh->computeGlobalPositions();
object->computeBoundaryBox(true);
}
ImplicitFuncCSG::ImplicitFuncCSG(const BBox<scalar,2>& bb,OP_TYPE op):_alpha(0.8f)
{
boost::shared_ptr<ImplicitFuncCSG> axis0(new ImplicitFuncCSG(op));
boost::shared_ptr<ImplicitFuncCSG> axis1(new ImplicitFuncCSG(op));
if(op == INTERSECT) {
axis0->_a.reset(new ImplicitFuncPlane(Vec3(bb._minC.x(),0.0f,0.0f),Vec3(-1.0f,0.0f,0.0f)));
axis0->_b.reset(new ImplicitFuncPlane(Vec3(bb._maxC.x(),0.0f,0.0f),Vec3( 1.0f,0.0f,0.0f)));
axis1->_a.reset(new ImplicitFuncPlane(Vec3(0.0f,bb._minC.y(),0.0f),Vec3(0.0f,-1.0f,0.0f)));
axis1->_b.reset(new ImplicitFuncPlane(Vec3(0.0f,bb._maxC.y(),0.0f),Vec3(0.0f, 1.0f,0.0f)));
} else {
axis0->_a.reset(new ImplicitFuncPlane(Vec3(bb._minC.x(),0.0f,0.0f),Vec3( 1.0f,0.0f,0.0f)));
axis0->_b.reset(new ImplicitFuncPlane(Vec3(bb._maxC.x(),0.0f,0.0f),Vec3(-1.0f,0.0f,0.0f)));
axis1->_a.reset(new ImplicitFuncPlane(Vec3(0.0f,bb._minC.y(),0.0f),Vec3(0.0f, 1.0f,0.0f)));
axis1->_b.reset(new ImplicitFuncPlane(Vec3(0.0f,bb._maxC.y(),0.0f),Vec3(0.0f,-1.0f,0.0f)));
}
_a=axis0;
_b=axis1;
}
void mouseMove(int x, int y)
{
if (buttonDown == -1) return;
int dx = x - lastX;
int dy = y - lastY;
lastX = x;
lastY = y;
if (buttonDown == GLUT_LEFT_BUTTON)
{
// rotate the model
// These vectors come from the (unusual) definition of the CHAI
// camera's rotation matrix:
//
// column 0: look
// column 2: up
// column 1: look x up
// Rotation around the horizontal camera axis
cVector3d axis1(0,1,0);
camera->getRot().mul(axis1);
object->rotate(axis1,1.0*(float)dy / 50.0);
// Rotation around the vertical camera axis
cVector3d axis2(0,0,1);
camera->getRot().mul(axis2);
object->rotate(axis2,(float)dx / 50.0);
object->computeGlobalPositions(true);
}
else
{
// move the model
cVector3d translation_vector =
(-1.0*(float)dy / 100.0) * camera->getUpVector() +
( 1.0*(float)dx / 100.0) * camera->getRightVector();
object->translate(translation_vector);
object->computeGlobalPositions(true);
}
}
bool IfcGeom::convert(const Ifc2x3::IfcCartesianTransformationOperator2DnonUniform::ptr l, gp_GTrsf2d& gtrsf) {
IN_CACHE(IfcCartesianTransformationOperator2DnonUniform,l,gp_GTrsf2d,gtrsf)
gp_Trsf2d trsf;
gp_Pnt origin;
IfcGeom::convert(l->LocalOrigin(),origin);
gp_Dir axis1 (1.,0.,0.);
if ( l->hasAxis1() ) IfcGeom::convert(l->Axis1(),axis1);
const gp_Ax2d ax2d (gp_Pnt2d(origin.X(),origin.Y()),gp_Dir2d(axis1.X(),axis1.Y()));
trsf.SetTransformation(ax2d);
trsf.Invert();
const double scale1 = l->hasScale() ? l->Scale() : 1.0f;
const double scale2 = l->hasScale2() ? l->Scale2() : scale1;
gtrsf = gp_GTrsf2d();
gtrsf.SetValue(1,1,scale1);
gtrsf.SetValue(2,2,scale2);
gtrsf.Multiply(trsf);
CACHE(IfcCartesianTransformationOperator2DnonUniform,l,gtrsf)
return true;
}
bool IfcGeom::convert(const Ifc2x3::IfcCartesianTransformationOperator3D::ptr l, gp_Trsf& trsf) {
IN_CACHE(IfcCartesianTransformationOperator3D,l,gp_Trsf,trsf)
gp_Pnt origin;
IfcGeom::convert(l->LocalOrigin(),origin);
gp_Dir axis1 (1.,0.,0.);
gp_Dir axis2 (0.,1.,0.);
gp_Dir axis3;
if ( l->hasAxis1() ) IfcGeom::convert(l->Axis1(),axis1);
if ( l->hasAxis2() ) IfcGeom::convert(l->Axis2(),axis2);
if ( l->hasAxis3() ) IfcGeom::convert(l->Axis3(),axis3);
else axis3 = axis1.Crossed(axis2);
gp_Ax3 ax3 (origin,axis3,axis1);
if ( axis2.Dot(ax3.YDirection()) < 0 ) ax3.YReverse();
trsf.SetTransformation(ax3);
trsf.Invert();
if ( l->hasScale() ) trsf.SetScaleFactor(l->Scale());
CACHE(IfcCartesianTransformationOperator3D,l,trsf)
return true;
}
//----------------------------------------------------------------------------
void RoughPlaneSolidBox::MoveBox ()
{
float x = (float)mModule.GetX();
float w = (float)mModule.GetW();
float xExt = (float)mModule.XLocExt;
float yExt = (float)mModule.YLocExt;
float zExt = (float)mModule.ZLocExt;
float sinPhi = (float)mModule.SinAngle;
float cosPhi = (float)mModule.CosAngle;
float theta = (float)mModule.GetTheta();
float sinTheta = Mathf::Sin(theta);
float cosTheta = Mathf::Cos(theta);
// Compute the box center.
APoint center(x, w*cosPhi - zExt*sinPhi, w*sinPhi + zExt*cosPhi);
// Compute the box orientation.
AVector axis0(cosTheta, -sinTheta*cosPhi, -sinTheta*sinPhi);
AVector axis1(sinTheta, +cosTheta*cosPhi, +cosTheta*sinPhi);
AVector axis2(0.0f, -sinPhi, cosPhi);
// Keep the box from sliding below the ground.
float zRadius =
xExt*Mathf::FAbs(axis0.Z()) +
yExt*Mathf::FAbs(axis1.Z()) +
zExt*Mathf::FAbs(axis2.Z());
if (center.Z() >= zRadius)
{
// Update the box.
mBox->LocalTransform.SetTranslate(center);
mBox->LocalTransform.SetRotate(HMatrix(axis0, axis1, axis2,
APoint::ORIGIN, true));
mBox->Update();
}
else
{
mDoUpdate = false;
}
}
static void __DecomposeMatrices(float *matrices, size_t count,
std::vector< shared_ptr <GLTFBufferView> > &TRSBufferViews) {
size_t translationBufferSize = sizeof(float) * 3 * count;
size_t rotationBufferSize = sizeof(float) * 4 * count;
size_t scaleBufferSize = sizeof(float) * 3 * count;
float *translationData = (float*)malloc(translationBufferSize);
float *rotationData = (float*)malloc(rotationBufferSize);
float *scaleData = (float*)malloc(scaleBufferSize);
shared_ptr <GLTF::GLTFBufferView> translationBufferView = createBufferViewWithAllocatedBuffer(translationData, 0, translationBufferSize, true);
shared_ptr <GLTF::GLTFBufferView> rotationBufferView = createBufferViewWithAllocatedBuffer(rotationData, 0, rotationBufferSize, true);
shared_ptr <GLTF::GLTFBufferView> scaleBufferView = createBufferViewWithAllocatedBuffer(scaleData, 0, scaleBufferSize, true);
float *previousRotation = 0;
for (size_t i = 0 ; i < count ; i++) {
float *m = matrices;
COLLADABU::Math::Matrix4 mat;
mat.setAllElements(m[0], m[1], m[2], m[3],
m[4], m[5], m[6], m[7],
m[8], m[9], m[10], m[11],
m[12], m[13], m[14], m[15] );
decomposeMatrix(mat, translationData, rotationData, scaleData);
//make sure we export the short path from orientations
if (0 != previousRotation) {
COLLADABU::Math::Vector3 axis1(previousRotation[0], previousRotation[1], previousRotation[2]);
COLLADABU::Math::Vector3 axis2(rotationData[0], rotationData[1], rotationData[2]);
COLLADABU::Math::Quaternion key1;
COLLADABU::Math::Quaternion key2;
key1.fromAngleAxis(previousRotation[3], axis1);
key2.fromAngleAxis(rotationData[3], axis2);
COLLADABU::Math::Real cosHalfTheta = key1.dot(key2);
if (cosHalfTheta < 0) {
key2.x = -key2.x;
key2.y = -key2.y;
key2.z = -key2.z;
key2.w = -key2.w;
COLLADABU::Math::Real angle;
key2.toAngleAxis(angle, axis2);
rotationData[3] = (float)angle;
rotationData[0] = (float)axis2.x;
rotationData[1] = (float)axis2.y;
rotationData[2] = (float)axis2.z;
key2.fromAngleAxis(rotationData[3], axis2);
//FIXME: this needs to be refined, we ensure continuity here, but assume in clockwise order
cosHalfTheta = key1.dot(key2);
if (cosHalfTheta < 0) {
rotationData[3] += (float)(2. * 3.14159265359);
key2.fromAngleAxis(rotationData[3], axis2);
}
}
}
previousRotation = rotationData;
translationData += 3;
rotationData += 4;
scaleData += 3;
matrices += 16;
}
/*
rotationData = (float*)rotationBufferView->getBufferDataByApplyingOffset();
for (size_t i = 0 ; i < count ; i++) {
printf("rotation at:%d %f %f %f %f\n", i,
rotationData[0],rotationData[1],rotationData[2],rotationData[3]);
rotationData += 4;
}
*/
TRSBufferViews.push_back(translationBufferView);
TRSBufferViews.push_back(rotationBufferView);
TRSBufferViews.push_back(scaleBufferView);
}
void CustomControlledBallAndSocket::SubmitConstraints (dFloat timestep, int threadIndex)
{
dMatrix matrix0;
dMatrix matrix1;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix (matrix0, matrix1);
// Restrict the movement on the pivot point along all tree orthonormal direction
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_front[0]);
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_up[0]);
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_right[0]);
#if 0
dVector euler0;
dVector euler1;
dMatrix localMatrix (matrix0 * matrix1.Inverse());
localMatrix.GetEulerAngles(euler0, euler1);
AngularIntegration pitchStep0 (AngularIntegration (euler0.m_x) - m_pitch);
AngularIntegration pitchStep1 (AngularIntegration (euler1.m_x) - m_pitch);
if (dAbs (pitchStep0.GetAngle()) > dAbs (pitchStep1.GetAngle())) {
euler0 = euler1;
}
dVector euler (m_pitch.Update (euler0.m_x), m_yaw.Update (euler0.m_y), m_roll.Update (euler0.m_z), 0.0f);
for (int i = 0; i < 3; i ++) {
dFloat error = m_targetAngles[i] - euler[i];
if (dAbs (error) > (0.125f * 3.14159213f / 180.0f) ) {
dFloat angularStep = dSign(error) * m_angulaSpeed * timestep;
if (angularStep > 0.0f) {
if (angularStep > error) {
angularStep = error * 0.5f;
}
} else {
if (angularStep < error) {
angularStep = error * 0.5f;
}
}
euler[i] = euler[i] + angularStep;
}
}
dMatrix p0y0r0 (dPitchMatrix(euler[0]) * dYawMatrix(euler[1]) * dRollMatrix(euler[2]));
dMatrix baseMatrix (p0y0r0 * matrix1);
dMatrix rotation (matrix0.Inverse() * baseMatrix);
dQuaternion quat (rotation);
if (quat.m_q0 > dFloat (0.99995f)) {
dVector euler0;
dVector euler1;
rotation.GetEulerAngles(euler0, euler1);
NewtonUserJointAddAngularRow(m_joint, euler0[0], &rotation[0][0]);
NewtonUserJointAddAngularRow(m_joint, euler0[1], &rotation[1][0]);
NewtonUserJointAddAngularRow(m_joint, euler0[2], &rotation[2][0]);
} else {
dMatrix basis (dGrammSchmidt (dVector (quat.m_q1, quat.m_q2, quat.m_q3, 0.0f)));
NewtonUserJointAddAngularRow (m_joint, 2.0f * dAcos (quat.m_q0), &basis[0][0]);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis[1][0]);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis[2][0]);
}
#else
matrix1 = m_targetRotation * matrix1;
dQuaternion localRotation(matrix1 * matrix0.Inverse());
if (localRotation.DotProduct(m_targetRotation) < 0.0f) {
localRotation.Scale(-1.0f);
}
dFloat angle = 2.0f * dAcos(localRotation.m_q0);
dFloat angleStep = m_angulaSpeed * timestep;
if (angleStep < angle) {
dVector axis(dVector(localRotation.m_q1, localRotation.m_q2, localRotation.m_q3, 0.0f));
axis = axis.Scale(1.0f / dSqrt(axis % axis));
// localRotation = dQuaternion(axis, angleStep);
}
dVector axis (matrix1.m_front * matrix1.m_front);
dVector axis1 (matrix1.m_front * matrix1.m_front);
//dFloat sinAngle;
//dFloat cosAngle;
//CalculatePitchAngle (matrix0, matrix1, sinAngle, cosAngle);
//float xxxx = dAtan2(sinAngle, cosAngle);
//float xxxx1 = dAtan2(sinAngle, cosAngle);
dQuaternion quat(localRotation);
if (quat.m_q0 > dFloat(0.99995f)) {
// dAssert (0);
/*
dVector euler0;
dVector euler1;
rotation.GetEulerAngles(euler0, euler1);
NewtonUserJointAddAngularRow(m_joint, euler0[0], &rotation[0][0]);
NewtonUserJointAddAngularRow(m_joint, euler0[1], &rotation[1][0]);
NewtonUserJointAddAngularRow(m_joint, euler0[2], &rotation[2][0]);
*/
} else {
dMatrix basis(dGrammSchmidt(dVector(quat.m_q1, quat.m_q2, quat.m_q3, 0.0f)));
NewtonUserJointAddAngularRow(m_joint, -2.0f * dAcos(quat.m_q0), &basis[0][0]);
NewtonUserJointAddAngularRow(m_joint, 0.0f, &basis[1][0]);
NewtonUserJointAddAngularRow(m_joint, 0.0f, &basis[2][0]);
}
#endif
}
//---------------------------------------------------------
void Poly3D::SortPoints(const DVec& cent)
//---------------------------------------------------------
{
// Sort the points by angle from cent which is
// a point in the plane of the polygon. This
// is done in a counter-clockwise direction.
// If less than 2 points, no need to sort
if (m_N < 2) {
return;
}
// create local cartesian axis
DVec ref1,ref2,nor,axis1,axis2,vI,vJ,tmp;
// wrap DM with DMat for utility routines
int Nr=m_xyz.num_rows(), Nc=m_xyz.num_cols();
DMat t_xyz; t_xyz.borrow(Nr, Nc, m_xyz.data());
t_xyz.get_col(1) - cent; ref1 /= ref1.norm();
// get two lines in the plane
ref1 = t_xyz.get_col(1) - cent; ref1 /= ref1.norm();
ref2 = t_xyz.get_col(2) - cent; ref2 /= ref2.norm();
// normal to the plane (norm = ref1 X ref2)
nor(1) = ref1(2)*ref2(3) - ref1(3)*ref2(2);
nor(2) = ref1(3)*ref2(1) - ref1(1)*ref2(3);
nor(3) = ref1(1)*ref2(2) - ref1(2)*ref2(1);
nor /= nor.norm();
// axis definition
axis1 = ref1;
// axis2 = norm x axis1
axis2(1) = nor(2)*axis1(3) - nor(3)*axis1(2);
axis2(2) = nor(3)*axis1(1) - nor(1)*axis1(3);
axis2(3) = nor(1)*axis1(2) - nor(2)*axis1(1);
double costhetaI,sinthetaI, costhetaJ,sinthetaJ, thetaI,thetaJ;
for (int i=1; i<=m_N; ++i) {
for (int j=(i+1); j<=m_N; ++j) {
vI = t_xyz.get_col(i) - cent;
vJ = t_xyz.get_col(j) - cent;
costhetaI = vI.inner(axis1);
sinthetaI = vI.inner(axis2);
costhetaJ = vJ.inner(axis1);
sinthetaJ = vJ.inner(axis2);
thetaI = atan2(sinthetaI, costhetaI);
thetaJ = atan2(sinthetaJ, costhetaJ);
// sort minimum angle difference first
if (thetaJ < thetaI)
{
// swap I and J
//t_xyz(All, [i j]) = t_xyz(All, [j i]);
tmp = t_xyz.get_col(i); // copy column i
t_xyz.set_col(i, t_xyz.get_col(j)); // overwrite col i
t_xyz.set_col(j, tmp); // overwrite col j
}
}
}
}
void Camera::move(char c, const std::vector<Entity> &entities,
const Map &map, int col, int row) {
Eigen::Quaternionf q1;
Eigen::Vector3f axis1(0.0f, 1.0f, 0.0f);
q1 = Eigen::AngleAxisf(rotations(0), axis1);
Eigen::Matrix4f R1 = Eigen::Matrix4f::Identity();
R1.block<3,3>(0,0) = q1.toRotationMatrix();
Eigen::Vector3f last_valid_location = translations;
Eigen::Vector3f point;
switch (c) {
case 's':
point = Eigen::Vector3f(0.0f, 0.0f, 1.0f);
break;
case 'd':
point = Eigen::Vector3f(-1.0f, 0.0f, 0.0f);
break;
case 'w':
point = Eigen::Vector3f(0.0f, 0.0f, -1.0f);
break;
case 'a':
point = Eigen::Vector3f(1.0f, 0.0f, 0.0f);
break;
default:
assert(0);
}
point = R1.block<3,3>(0,0) * point;
translations(0) += (point(0) * tfactor);
translations(2) -= (point(2) * tfactor);
if (col >= 0 && row >= 0) {
Neighbors neighbors = map.getNearbyWalls(col, row);
bool reset_x = false, reset_z = false;
if ((neighbors.up && this->collides(*neighbors.up)) ||
(neighbors.down && this->collides(*neighbors.down))) {
// std::cout << "COLLIDES Z" << std::endl;
reset_z = true;
}
if ((neighbors.left && this->collides(*neighbors.left)) ||
(neighbors.right && this->collides(*neighbors.right))) {
// std::cout << "COLLIDES X" << std::endl;
reset_x = true;
}
for (auto it = entities.begin(); it != entities.end(); it++) {
if (this->collides(*it)) {
reset_x = reset_z = true;
break;
}
}
if (reset_x || reset_z) {
translations = last_valid_location;
if (reset_x) {
point(0) = 0;
}
if (reset_z) {
point(2) = 0;
}
translations(0) += (point(0) * tfactor);
translations(2) -= (point(2) * tfactor);
}
}
}
void Camera::move(char c, const Level &level_one,
const Map &map, int col, int row, float mov) {
Eigen::Quaternionf q1;
Eigen::Vector3f axis1(0.0f, 1.0f, 0.0f);
q1 = Eigen::AngleAxisf(rotations(0), axis1);
Eigen::Matrix4f R1 = Eigen::Matrix4f::Identity();
R1.block<3,3>(0,0) = q1.toRotationMatrix();
Eigen::Vector3f last_valid_location = translations;
Eigen::Vector3f point;
switch (c) {
case 's':
point = Eigen::Vector3f(0.0f, 0.0f, mov);
break;
case 'd':
point = Eigen::Vector3f(-mov, 0.0f, 0.0f);
break;
case 'w':
point = Eigen::Vector3f(0.0f, 0.0f, -mov);
break;
case 'a':
point = Eigen::Vector3f(mov, 0.0f, 0.0f);
break;
default:
assert(0);
}
point = R1.block<3,3>(0,0) * point;
translations(0) += (point(0) * tfactor);
translations(2) -= (point(2) * tfactor);
if (col >= 0 && row >= 0) {
Neighbors neighbors = map.getNearbyWalls(col, row);
bool reset_x = false, reset_z = false;
if ((neighbors.up && this->collides(*neighbors.up)) ||
(neighbors.down && this->collides(*neighbors.down))) {
// std::cout << "COLLIDES Z" << std::endl;
reset_z = true;
}
if ((neighbors.left && this->collides(*neighbors.left)) ||
(neighbors.right && this->collides(*neighbors.right))) {
// std::cout << "COLLIDES X" << std::endl;
reset_x = true;
}
for (int i = 0; i < level_one.getNumRooms(); ++i) {
std::vector<Entity> b_entities;
b_entities = (level_one.getRooms())[i]->boundaries;
for (auto it = b_entities.begin(); it != b_entities.end(); it++) {
if (this->collides(*it)) {
reset_x = reset_z = true;
i = level_one.getNumRooms();
break;
}
}
if (i != level_one.getNumRooms()) {
std::vector<Entity> t_entities;
t_entities = (level_one.getRooms())[i]->entities;
for (auto it = t_entities.begin(); it != t_entities.end(); it++) {
if (this->collides(*it)) {
reset_x = reset_z = true;
i = level_one.getNumRooms();
break;
}
}
}
}
if (reset_x || reset_z) {
translations = last_valid_location;
if (reset_x) {
point(0) = 0;
}
if (reset_z) {
point(2) = 0;
}
translations(0) += (point(0) * tfactor);
translations(2) -= (point(2) * tfactor);
}
else {
for (int i = 0; i < level_one.getNumRooms(); ++i) {
(level_one.getRooms())[i]->triggerRoom(vec2(col, row));
}
}
}
}
bool ChunkyBoneGeometry::CreateJoint(ChunkyPhysics* structure, PhysicsManager* physics, unsigned physics_fps) {
bool ok = false;
if (body_data_.parent_) {
if (GetBoneType() == kBonePosition) {
// Need not do jack. It's not a physical object.
ok = true;
} else if (body_data_.joint_type_ == kJointExclude) {
ok = physics->Attach(GetBodyId(), body_data_.parent_->GetBodyId());
} else if (body_data_.joint_type_ == kJointFixed) {
ok = physics->Attach(GetBodyId(), body_data_.parent_->GetBodyId());
} else if (body_data_.joint_type_ == kJointSuspendHinge || body_data_.joint_type_ == kJointHinge2) {
// Calculate axis from given euler angles.
vec3 suspension_axis(-1, 0, 0);
vec3 hinge_axis(0, 0, 1);
quat rotator;
rotator.SetEulerAngles(body_data_.parameter_[kParamEulerTheta], 0, body_data_.parameter_[kParamEulerPhi]);
suspension_axis = rotator*suspension_axis;
hinge_axis = rotator*hinge_axis;
joint_id_ = physics->CreateHinge2Joint(body_data_.parent_->GetBodyId(),
GetBodyId(), structure->GetTransformation(this).GetPosition(),
suspension_axis, hinge_axis);
physics->SetJointParams(joint_id_, body_data_.parameter_[kParamLowStop], body_data_.parameter_[kParamHighStop], 0);
physics->SetSuspension(joint_id_, 1/(float)physics_fps, body_data_.parameter_[kParamSpringConstant],
body_data_.parameter_[kParamSpringDamping]);
physics->SetAngularMotorRoll(joint_id_, 0, 0);
physics->SetAngularMotorTurn(joint_id_, 0, 0);
ok = true;
} else if (body_data_.joint_type_ == kJointHinge) {
// Calculate axis from given euler angles.
vec3 hinge_axis(0, 0, 1);
quat hinge_rotator;
hinge_rotator.SetEulerAngles(body_data_.parameter_[kParamEulerTheta], 0, body_data_.parameter_[kParamEulerPhi]);
hinge_axis = hinge_rotator*hinge_axis;
const xform& body_transform = structure->GetTransformation(this);
const vec3 anchor = body_transform.GetPosition() + GetOriginalOffset();
joint_id_ = physics->CreateHingeJoint(body_data_.parent_->GetBodyId(),
GetBodyId(), anchor, hinge_axis);
physics->SetJointParams(joint_id_, body_data_.parameter_[kParamLowStop], body_data_.parameter_[kParamHighStop], body_data_.bounce_);
physics->SetAngularMotorTurn(joint_id_, 0, 0);
//physics->GetAxis1(joint_id_, hinge_axis);
ok = true;
} else if (body_data_.joint_type_ == kJointSlider) {
// Calculate axis from given euler angles.
vec3 axis(0, 0, 1);
quat rotator;
rotator.SetEulerAngles(body_data_.parameter_[kParamEulerTheta], 0, body_data_.parameter_[kParamEulerPhi]);
axis = rotator*axis;
joint_id_ = physics->CreateSliderJoint(body_data_.parent_->GetBodyId(),
GetBodyId(), axis);
physics->SetJointParams(joint_id_, body_data_.parameter_[kParamLowStop], body_data_.parameter_[kParamHighStop], body_data_.bounce_);
physics->SetMotorTarget(joint_id_, 0, 0);
ok = true;
} else if (body_data_.joint_type_ == kJointUniversal) {
// Calculate axis from given euler angles.
vec3 axis1(0, 0, 1);
vec3 axis2(0, 1, 0);
quat rotator;
rotator.SetEulerAngles(body_data_.parameter_[kParamEulerTheta], 0, body_data_.parameter_[kParamEulerPhi]);
axis1 = rotator*axis1;
axis2 = rotator*axis2;
const xform& body_transform = structure->GetTransformation(this);
const vec3 anchor = body_transform.GetPosition() +
vec3(body_data_.parameter_[kParamOffsetX], body_data_.parameter_[kParamOffsetY], body_data_.parameter_[kParamOffsetZ]);
joint_id_ = physics->CreateUniversalJoint(body_data_.parent_->GetBodyId(),
GetBodyId(), anchor, axis1, axis2);
physics->SetJointParams(joint_id_, body_data_.parameter_[kParamLowStop], body_data_.parameter_[kParamHighStop], 0);
/*physics->SetJointParams(joint_id_, body_data_.parameter_[kParamLowStop], body_data_.parameter_[kParamHighStop], 0);
physics->SetSuspension(joint_id_, 1/(float)physics_fps, body_data_.parameter_[0],
body_data_.parameter_[1]);
physics->SetAngularMotorRoll(joint_id_, 0, 0);
physics->SetAngularMotorTurn(joint_id_, 0, 0);*/
ok = true;
} else if (body_data_.joint_type_ == kJointBall) {
const xform& body_transform = structure->GetTransformation(this);
const vec3 anchor = body_transform.GetPosition() +
vec3(body_data_.parameter_[kParamOffsetX], body_data_.parameter_[kParamOffsetY], body_data_.parameter_[kParamOffsetZ]);
joint_id_ = physics->CreateBallJoint(body_data_.parent_->GetBodyId(),
GetBodyId(), anchor);
/*physics->SetJointParams(joint_id_, body_data_.parameter_[kParamLowStop], body_data_.parameter_[kParamHighStop], 0);
physics->SetJointParams(joint_id_, body_data_.parameter_[kParamLowStop], body_data_.parameter_[kParamHighStop], 0);
physics->SetSuspension(joint_id_, 1/(float)physics_fps, body_data_.parameter_[0],
body_data_.parameter_[1]);
physics->SetAngularMotorRoll(joint_id_, 0, 0);
physics->SetAngularMotorTurn(joint_id_, 0, 0);*/
ok = true;
} else {
deb_assert(false);
}
} else {
deb_assert(body_data_.joint_type_ == kJointExclude);
ok = true;
}
deb_assert(ok);
return (ok);
}
