bool CLaser::GetFiringDir(EntityId weaponId, const IFireMode* pFireMode, Vec3& dir, const Vec3& probableHit, const Vec3& firingPos) { CWeapon* pParentWeapon = GetWeapon(); if (!m_laserBeam.IsLaserActivated()) return false; if(gEnv->bMultiplayer && pParentWeapon && pParentWeapon->IsZoomed()) { return false; } if(!probableHit.IsZero() && !firingPos.IsZero()) { dir = (probableHit - firingPos).GetNormalized(); return true; } if (pParentWeapon) { int slot = pParentWeapon->IsOwnerFP() ? eIGS_FirstPerson : eIGS_ThirdPerson; Matrix33 rotation = pParentWeapon->GetSlotHelperRotation(slot, "weapon_term", true); dir = rotation.GetColumn1(); return true; } return false; }
bool CLaser::GetProbableHit(EntityId weaponId, const IFireMode* pFireMode, Vec3& hit) { if (!m_laserBeam.IsLaserActivated()) return false; if(gEnv->bMultiplayer) { CWeapon* pParentWeapon = GetWeapon(); if (pParentWeapon && pParentWeapon->IsZoomed()) { return false; } } CWeapon* pWeapon = GetWeapon(); int slot = pWeapon->IsOwnerFP() ? eIGS_FirstPerson : eIGS_ThirdPerson; Vec3 lastBeamHit = m_laserBeam.GetLastHit(); Vec3 currentBeamPosition = pWeapon->GetSlotHelperPos(slot, "weapon_term", true); Matrix33 rotation = pWeapon->GetSlotHelperRotation(slot, "weapon_term", true); Vec3 currentBeamDirection = rotation.GetColumn1(); const CFireMode* pCFireMode = static_cast<const CFireMode*>(pFireMode); const CSingle* pSingle = crygti_cast<const CSingle*>(pCFireMode); if (pSingle && pSingle->GetShared()->fireparams.laser_beam_uses_spread) { currentBeamDirection = pSingle->ApplySpread(currentBeamDirection, pSingle->GetSpread()); } float distanceToLastHit = lastBeamHit.GetDistance(currentBeamPosition); hit = currentBeamPosition + currentBeamDirection * distanceToLastHit; return true; }
void getSigma(MATRIX33* dest,MATRIX33* F_in){ Matrix33 F(F_in); for(unsigned i=0;i<3;i++) F(i,i)+=1.0; double J=F.I3(); Matrix33 F_T=F.transpose(); Matrix33 C=F_T*F; double I_C =C.I1(); double II_C =C.I2(); double III_C=C.I3(); Matrix33 C_inv=C.inv(); Matrix33 I; I.identity(); Matrix33 c1_mat,c2_mat; { c1_mat=-1./3.*I_C*C_inv; for(unsigned i=0;i<3;i++) c1_mat(i,i)+=1.; c1_mat*=c1*pow(III_C,-1./3.); }{ c2_mat=I_C*I-C-2./3.*II_C*C_inv; c2_mat*=c2*pow(III_C,-2./3.); } Matrix33 S=c1_mat+c2_mat; Matrix33 tau=F*S*F_T; for(int i=0;i<3;i++) for(int j=0;j<3;j++) (*dest)(i,j)=tau(i,j)/J*2.; };
int main(int argc,char **argv) { GLUTWrapper::setApplication(new TestApp); GLUTWrapper::run(argc,argv); Matrix33 m(3,1,0,-1,2,2,5,0,-1); //Matrix33 m(3,3,6,0,1,2,-2,0,0); std::cout << "MATRIX:\n" << m << std::endl; std::cout << "MATRIX^-1:\n" << m.inverse() << std::endl; Matrix33 mxi = m * m.inverse(Matrix33::GAUSS_JORDAN); mxi.clean(); std::cout << "MATRIX x MATRIX^-1:\n" << mxi << std::endl; /*try { PPMImage img("abe_natsumi.ppm"); } catch(Exception e) { std::cout << e.what() << std::endl; }*/ //inversion_tests(); //big_matrix_inverse(); //test(); while(!_kbhit()) ; return 0; }
//------------------------------------------------------------------------ const Matrix33 &CItem::GetSlotHelperRotation(int slot, const char *helper, bool worldSpace, bool relative) { static Matrix33 rotation; rotation.SetIdentity(); IEntity* pEntity = GetEntity(); if(!pEntity) return rotation; SEntitySlotInfo info; if (pEntity->GetSlotInfo(slot, info)) { if (info.pStatObj) { IStatObj *pStatObj = info.pStatObj; rotation = Matrix33(pStatObj->GetHelperTM(helper)); rotation.OrthonormalizeFast(); rotation = Matrix33(GetEntity()->GetSlotLocalTM(slot, false))*rotation; } else if (info.pCharacter) { ICharacterInstance *pCharacter = info.pCharacter; if(!pCharacter) return rotation; IAttachment* pAttachment = pCharacter->GetIAttachmentManager()->GetInterfaceByName(helper); if(pAttachment) { rotation = Matrix33(worldSpace ? pAttachment->GetAttWorldAbsolute().q : pAttachment->GetAttModelRelative().q); return rotation; } else { ICharacterModelSkeleton* pICharacterModelSkeleton = pCharacter->GetICharacterModel()->GetICharacterModelSkeleton(); ISkeletonPose* pSkeletonPose = pCharacter->GetISkeletonPose(); int16 id = pICharacterModelSkeleton->GetJointIDByName(helper); if (id > -1) { rotation = relative ? Matrix33(pSkeletonPose->GetRelJointByID(id).q) : Matrix33(pSkeletonPose->GetAbsJointByID(id).q); } } if (!relative) { rotation = Matrix33(pEntity->GetSlotLocalTM(slot, false)) * rotation; } } } if (worldSpace) { rotation = Matrix33(pEntity->GetWorldTM()) * rotation; } return rotation; }
void OpenGLContext::scale( VGfloat sx, VGfloat sy ) { Matrix33* active = getActiveMatrix(); Matrix33 scale; scale.setIdentity(); scale.setScale( sx, sy ); Matrix33 tmp; Matrix33::multiply( tmp, scale, *active ); active->copy( tmp ); loadGLMatrix(); }
inline Matrix33 horizontal( const Matrix33& src ) { Matrix33 dest = src; dest( 1, 1 ) = 1; dest( 0, 1 ) = 0; dest( 2, 1 ) = 0; dest( 1, 0 ) = 0; dest( 1, 2 ) = 0; dest.Orthonormalize(); return dest; }
void Exporter::objectTransform(QuaternionXYZW &rot, Vector3 &trans, INode *node, TimeValue t, bool local) { Matrix33 rm; objectTransform(rm, trans, node, t, local); Quaternion q = rm.AsQuaternion(); rot.x = q.x; rot.y = q.y; rot.z = q.z; rot.w = q.w; }
void OpenGLContext::multiply( const VGfloat* t ) { Matrix33 m; for ( int x = 0; x < 3; x++ ) { for ( int y = 0; y < 3; y++ ) { m.set( y, x, t[(y*3)+x] ); } } Matrix33* active = getActiveMatrix(); active->postMultiply( m ); loadGLMatrix(); }
//------------------------------------------------------------------------ const Matrix33 &CItem::GetSlotHelperRotation(int slot, const char *helper, bool worldSpace, bool relative) { // if mounted force the slot to be 1st person if(m_stats.mounted) slot=eIGS_FirstPerson; static Matrix33 rotation; rotation.SetIdentity(); IEntity *pEntity = GetEntity(); if(!pEntity) return rotation; SEntitySlotInfo info; if(pEntity->GetSlotInfo(slot, info)) { if(info.pStatObj) { IStatObj *pStatObj = info.pStatObj; rotation = Matrix33(pStatObj->GetHelperTM(helper)); rotation.OrthonormalizeFast(); rotation = Matrix33(GetEntity()->GetSlotLocalTM(slot, false))*rotation; } else if(info.pCharacter) { ICharacterInstance *pCharacter = info.pCharacter; if(!pCharacter) return rotation; int16 id = pCharacter->GetISkeletonPose()->GetJointIDByName(helper); // if (id > -1) rotation = Matrix33(pCharacter->GetISkeleton()->GetAbsJMatrixByID(id)); if(id > -1) { if(relative) rotation = Matrix33(pCharacter->GetISkeletonPose()->GetRelJointByID(id).q); else rotation = Matrix33(pCharacter->GetISkeletonPose()->GetAbsJointByID(id).q); } if(!relative) rotation = Matrix33(pEntity->GetSlotLocalTM(slot, false))*rotation; } } if(worldSpace) rotation=Matrix33(pEntity->GetWorldTM())*rotation; return rotation; }
void CRotatableTurret::Update( float dt ) { UpdateAimInfo(); Matrix34 init_world_pose = m_ParentWorldPose * m_MountLocalPose; // Why calculate on local space? // - world pose of the turret keeps changes if the owner entity keeps moving. // Vector3 vTargetDirection = m_vAimDirection; Vector3 vLocalTTableAxis = m_MountLocalPose.matOrient.GetColumn(1); // y-axis Vector3 vLocalTargetDir; init_world_pose.InvTransform( vLocalTargetDir, m_vAimDirection ); Vector3 vLocalTgtDirOnTTable = vLocalTargetDir - vLocalTTableAxis * Vec3Dot( vLocalTTableAxis, vLocalTargetDir ); Vec3Normalize( vLocalTgtDirOnTTable, vLocalTgtDirOnTTable ); Matrix33 matLocalTargetRotation; matLocalTargetRotation.SetColumn( 0, Vec3Cross( vLocalTTableAxis, vLocalTgtDirOnTTable ) ); matLocalTargetRotation.SetColumn( 1, vLocalTTableAxis ); matLocalTargetRotation.SetColumn( 2, vLocalTgtDirOnTTable ); matLocalTargetRotation.Orthonormalize(); m_LocalTurnTableOrient.target.FromRotationMatrix( matLocalTargetRotation ); // calc how much the gun tube needs to be elevated (pitch rotation) float target_pitch = Vec3GetAngleBetween( vLocalTargetDir, vLocalTgtDirOnTTable ); Limit( target_pitch, 0.0f, deg_to_rad(85.0f) ); m_LocalGunTubePitchAngle.target = target_pitch; // update critical damping m_LocalGunTubePitchAngle.Update( dt ); m_LocalTurnTableOrient.Update( dt ); // vLocalTTableRightDir = m_LocalTurnTableOrient.current.ToRotationMatrix().GetColumn(0); // update local poses Matrix34 local_mount_pose, local_gun_tube_pose; local_mount_pose = Matrix34( m_MountLocalPose.vPosition, m_LocalTurnTableOrient.current.ToRotationMatrix() ); // local_gun_tube_pose = Matrix34( m_GunLocalPose.vPosition, Matrix33RotationAxis( m_LocalGunTubePitchAngle.current, vLocalTTableRightDir ) ); local_gun_tube_pose = Matrix34( m_GunLocalPose.vPosition, Matrix33RotationAxis( m_LocalGunTubePitchAngle.current, m_GunLocalPose.matOrient.GetColumn(0) ) ); m_MountWorldPose = m_ParentWorldPose * local_mount_pose; m_GunWorldPose = m_MountWorldPose * local_gun_tube_pose; }
Dvoid Camera::ViewMatrixCalculate() { Matrix33 rotate; rotate.SetColumns(m_vDir[CD_RIGHT], m_vDir[CD_UP], -m_vDir[CD_FRONT]); rotate.Transpose(); m_matCam[CM_VIEW].Rotation(rotate); // set translation (rotate into view space) Vector3 invEye = -(rotate* m_vPos); m_matCam[CM_VIEW](0, 3) = invEye.x; m_matCam[CM_VIEW](1, 3) = invEye.y; m_matCam[CM_VIEW](2, 3) = invEye.z; }
bool CollisionBoxComponent::CheckCollision(const Vector2<float>& aPosition) const { Matrix33<float> localSpace = myParent->GetSpace(); if (myLineVolume.Inside(aPosition - (myPosition + localSpace.GetPosition())) == true) { return true; } else { return false; } }
void CoreLayer2D_Imp::DrawAdditionalObjects() { #if __CULLING_2D__ //一時的にエフェクトは無条件に描画(本来ここではない) for (auto& x : m_objects) { if (x->GetIsAlive() && x->GetObjectType() == Object2DType::Effect) { x->Draw(m_renderer); } } #endif for (auto& sprite : sprites) { m_renderer->AddSprite( sprite.pos.data(), sprite.col.data(), sprite.uv.data(), sprite.Texture_.get(), sprite.AlphaBlend_, sprite.Priority); } for (auto& text : texts) { Matrix33 matP; Matrix33 mat; mat.SetTranslation(text.Position_.X, text.Position_.Y); m_renderer->AddText( matP, mat, Vector2DF(), false, false, text.Color_, text.Font_.get(), text.Text_.c_str(), text.WritingDirection_, text.AlphaBlend_, text.Priority_, 0, 0); } }
void InitIncrementalPose::initPose(const Matrix33& R, const Vector31& T, Pose& pose) const { pose.sett(RealPoint3D<double>(T.GetX(), T.GetY(), T.GetZ())); Matrix<double>& pR = pose.R(); pR[0][0] = R.Get00(); pR[1][0] = R.Get01(); pR[2][0] = R.Get02(); pR[0][1] = R.Get10(); pR[1][1] = R.Get11(); pR[2][1] = R.Get12(); pR[0][2] = R.Get20(); pR[1][2] = R.Get21(); pR[2][2] = R.Get22(); pose.calcEulerAngles(); pose.setorientationSynchronWithAngles(true); pose.setderivationsSynchronWithAngles(false); }
void readOOBB(std::string filePath, Vector3& minP, Vector3& maxP, Matrix33& R_KI, Vector3List& pointList) { auto v = getPointsFromFile3D(filePath); if(v.size() != 5 + 8) { ApproxMVBB_ERRORMSG("Wrong number of points in OOBB file: " << filePath << " points: " << v.size()) } minP = v[0]; maxP = v[1]; R_KI.row(0) = v[2]; R_KI.row(1) = v[3]; R_KI.row(2) = v[4]; pointList.resize(8); for(unsigned int i = 0; i < 8; ++i) { pointList[i] = v[5 + i]; } }
void Exporter::convertMatrix(Matrix33 &dst, const Matrix3 &src) { Point3 r0 = src.GetRow(0); Point3 r1 = src.GetRow(1); Point3 r2 = src.GetRow(2); dst.Set(r0.x, r0.y, r0.z, r1.x, r1.y, r1.z, r2.x, r2.y, r2.z); }
bool CustomizedJointPath::calcInverseKinematics(const Vector3& end_p, const Matrix33& end_R) { bool solved; if(ikTypeId == 0 || isBestEffortIKMode){ solved = JointPath::calcInverseKinematics(end_p, end_R); } else { std::vector<double> qorg(numJoints()); for(int i=0; i < numJoints(); ++i){ qorg[i] = joint(i)->q; } const Link* targetLink = endLink(); const Link* baseLink_ = baseLink(); Vector3 p_relative; Matrix33 R_relative; if(!isCustomizedIkPathReversed){ p_relative.noalias() = baseLink_->R.transpose() * (end_p - baseLink_->p); R_relative.noalias() = baseLink_->R.transpose() * end_R; } else { p_relative.noalias() = end_R.transpose() * (baseLink_->p - end_p); R_relative.noalias() = end_R.transpose() * baseLink_->R; } solved = body->customizerInterface-> calcAnalyticIk(body->customizerHandle, ikTypeId, p_relative, R_relative); if(solved){ calcForwardKinematics(); Vector3 dp(end_p - targetLink->p); Vector3 omega(omegaFromRot(targetLink->R.transpose() * end_R)); double errsqr = dp.dot(dp) + omega.dot(omega); if(errsqr < maxIKErrorSqr){ solved = true; } else { solved = false; for(int i=0; i < numJoints(); ++i){ joint(i)->q = qorg[i]; } calcForwardKinematics(); } } } return solved; }
//==================================================================== // DebugDrawWireFOVCone //==================================================================== void CAIDebugRenderer::DrawWireFOVCone(const Vec3& vPos, const Vec3& vDir, float fRadius, float fFOV, const ColorB& color) { const unsigned int npts = 32; const unsigned int npts2 = 16; Vec3 points[npts]; Vec3 pointsx[npts2]; Vec3 pointsy[npts2]; Matrix33 base; base.SetRotationVDir(vDir); float coneRadius = sinf(fFOV) * fRadius; float coneHeight = cosf(fFOV) * fRadius; for (unsigned int i = 0; i < npts; i++) { float a = ((float)i / (float)npts) * gf_PI2; float rx = cosf(a) * coneRadius; float ry = sinf(a) * coneRadius; points[i] = vPos + base.TransformVector(Vec3(rx, coneHeight, ry)); } for (unsigned int i = 0; i < npts2; i++) { float a = -fFOV + ((float)i / (float)(npts2-1)) * (fFOV*2); float rx = sinf(a) * fRadius; float ry = cosf(a) * fRadius; pointsx[i] = vPos + base.TransformVector(Vec3(rx, ry, 0)); pointsy[i] = vPos + base.TransformVector(Vec3(0, ry, rx)); } DrawPolyline(points, npts, true, color); DrawPolyline(pointsx, npts2, false, color); DrawPolyline(pointsy, npts2, false, color); DrawLine(points[0], color, vPos, color); DrawLine(points[npts/4], color, vPos, color); DrawLine(points[npts/2], color, vPos, color); DrawLine(points[npts/2+npts/4], color, vPos, color); }
//world -> view Matrix44 TransformHelper::CreateView(const Vector3& eye, const Vector3& lookAt, const Vector3& up) { // compute view vectors Vector3 view = lookAt - eye; Vector3 right; Vector3 viewUp; view.Normalize(); right = view.Cross(up); right.Normalize(); viewUp = right.Cross(view); viewUp.Normalize(); // now set up matrices // base rotation matrix Matrix33 rotate; rotate.SetColumns(right, viewUp, -view); // view->world transform // set rotation //Matrix44 mViewToWorldMatrix; //mViewToWorldMatrix.Rotation(rotate); //// set translation (eye position) //mViewToWorldMatrix(0, 3) = eye.x; //mViewToWorldMatrix(1, 3) = eye.y; //mViewToWorldMatrix(2, 3) = eye.z; // world->view transform // set rotation Matrix44 mWorldToViewMatrix; rotate.Transpose(); mWorldToViewMatrix.Rotation(rotate); // set translation (rotate into view space) Vector3 invEye = -(rotate*eye); mWorldToViewMatrix(0, 3) = invEye.x; mWorldToViewMatrix(1, 3) = invEye.y; mWorldToViewMatrix(2, 3) = invEye.z; return mWorldToViewMatrix; }
// Helpful print functions void print_matrix(Matrix33 const &_matrix) { DebugLogPrint("MATRIX:\n"); for (int i = 0; i < 3; ++i) { DebugLogPrint("ROW%d:", i); for (int j = 0; j < 3; ++j) { DebugLogPrint(" %f ", _matrix.values[i][j]); } DebugLogPrint("\n"); } DebugLogPrint("DETERMINANT: %f\n", _matrix.Determinant()); }
void OpenGLContext::rotate( VGfloat angle ) { Matrix33* active = getActiveMatrix(); Matrix33 rotate; rotate.setRotation( radians( angle ) ); Matrix33 tmp; tmp.setIdentity(); Matrix33::multiply( tmp, rotate, *active ); active->copy( tmp ); loadGLMatrix(); }
void OpenGLContext::translate( VGfloat x, VGfloat y ) { Matrix33* active = getActiveMatrix(); Matrix33 translate; translate.setTranslate( x, y ); Matrix33 tmp; tmp.setIdentity(); Matrix33::multiply( tmp, translate, *active ); active->copy( tmp ); loadGLMatrix(); }
void CVehicleMovementHelicopter::ProcessAI(const float deltaTime) { FUNCTION_PROFILER( GetISystem(), PROFILE_GAME ); CryAutoCriticalSection lk(m_lock); SVehiclePhysicsStatus* physStatus = &m_physStatus[k_physicsThread]; if (m_arcade.m_handling.maxSpeedForward>0.f) // Use the new handling code { //ResetActions(); m_movementAction.Clear(); m_movementAction.isAI = true; SVehiclePhysicsHelicopterProcessAIParams params; params.pPhysStatus = physStatus; params.pInputAction = &m_inputAction; params.pAiRequest = &m_aiRequest; params.dt = deltaTime; params.aiRequiredVel = m_CurrentVel; params.aiCurrentSpeed = m_CurrentSpeed; params.aiYawResponseScalar = m_yawResponseScalar; m_yawResponseScalar = 1.f; // Use helper class to process the AI input // It will return a requested velocity, and change the input m_arcade.ProcessAI(params); // Get the output velocity m_CurrentVel = params.aiRequiredVel; m_CurrentSpeed = params.aiCurrentSpeed; return; } ////////////////////// OLD DEPRECATED CODE :( ////////////////////////////////// m_movementAction.Clear(); ResetActions(); // Our current state const Vec3 worldPos = physStatus->pos; const Matrix33 worldMat( physStatus->q); const Matrix33 localMat( physStatus->q.GetInverted()); const Ang3 worldAngles = Ang3::GetAnglesXYZ(worldMat); const Ang3 localAngles = Ang3::GetAnglesXYZ(localMat); const Vec3 currentVel = physStatus->v; const Vec3 currentVel2D(currentVel.x, currentVel.y, 0.0f); m_CurrentSpeed = m_CurrentVel.len(); //currentVel.len(); float currentSpeed2d = currentVel2D.len(); // +ve direction mean rotation anti-clocwise about the z axis - 0 means along y float currentDir = worldAngles.z; // to avoid singularity const Vec3 vWorldDir = worldMat.GetRow(1); const Vec3 vSideWays = worldMat.GetRow(0); const Vec3 vWorldDir2D = Vec3( vWorldDir.x, vWorldDir.y, 0.0f ).GetNormalizedSafe(); // Our inputs float desiredSpeed = m_aiRequest.HasDesiredSpeed() ? m_aiRequest.GetDesiredSpeed() : 0.0f; Limit(desiredSpeed, -m_maxSpeed, m_maxSpeed); const Vec3 desiredMoveDir = m_aiRequest.HasMoveTarget() ? (m_aiRequest.GetMoveTarget() - worldPos).GetNormalizedSafe() : vWorldDir; Vec3 desiredMoveDir2D = Vec3(desiredMoveDir.x, desiredMoveDir.y, 0.0f); desiredMoveDir2D = desiredMoveDir2D.GetNormalizedSafe(desiredMoveDir2D); const Vec3 desiredVel = desiredMoveDir * desiredSpeed; const Vec3 desiredVel2D(desiredVel.x, desiredVel.y, 0.0f); Vec3 desiredLookDir(desiredMoveDir); if (m_aiRequest.HasDesiredBodyDirectionAtTarget()) { desiredLookDir = m_aiRequest.GetDesiredBodyDirectionAtTarget().GetNormalizedSafe(desiredMoveDir); } else if (m_aiRequest.HasLookTarget()) { desiredLookDir = (m_aiRequest.GetLookTarget() - worldPos).GetNormalizedSafe(desiredMoveDir); } //const Vec3 desiredLookDir = m_aiRequest.HasLookTarget() ? (m_aiRequest.GetLookTarget() - worldPos).GetNormalizedSafe() : desiredMoveDir; const Vec3 desiredLookDir2D = Vec3(desiredLookDir.x, desiredLookDir.y, 0.0f).GetNormalizedSafe(vWorldDir2D); Vec3 prediction = m_aiRequest.HasBodyTarget() ? m_aiRequest.GetBodyTarget() : ZERO; prediction = (prediction.IsEquivalent(ZERO)) ? desiredMoveDir2D : prediction - worldPos; prediction.z = 0.0f; float speedLimit = prediction.GetLength2D(); if(speedLimit > 0.0f) { prediction *= 1.0f / speedLimit; } Vec3 tempDir = currentVel2D.IsEquivalent(ZERO) ? localMat.GetRow(1) : currentVel2D; tempDir.z = 0.0f; tempDir.NormalizeFast(); float dotProd = tempDir.dot(prediction); Limit(dotProd, FLT_EPSILON, 1.0f); float accel = m_enginePowerMax * min(2.0f, 1.0f / dotProd); // * dotProd; if (!m_aiRequest.HasDesiredBodyDirectionAtTarget()) { dotProd *= dotProd; dotProd *= dotProd; float tempf = min(max(speedLimit * speedLimit, 2.0f), m_maxSpeed * dotProd); Limit(desiredSpeed, -tempf, tempf); } else if (dotProd < 0.0125f) { Limit(desiredSpeed, -m_maxSpeed * 0.25f, m_maxSpeed * 0.25f); } float posNeg = (float)__fsel(desiredSpeed - m_CurrentSpeed, 1.0f, -5.0f); if (desiredVel2D.GetLengthSquared() > FLT_EPSILON) { m_CurrentSpeed = m_CurrentSpeed + posNeg * accel * deltaTime; } else { m_CurrentSpeed = m_CurrentSpeed + posNeg * accel * deltaTime; } if (posNeg > 0.0f && m_CurrentSpeed > desiredSpeed) { m_CurrentSpeed = desiredSpeed; } else if (posNeg < 0.0f && m_CurrentSpeed < desiredSpeed) { m_CurrentSpeed = desiredSpeed; } // ---------------------------- Rotation ---------------------------- float desiredDir = (desiredLookDir2D.GetLengthSquared() > 0.0f) ? atan2f(-desiredLookDir2D.x, desiredLookDir2D.y) : atan2f(-vWorldDir2D.x, vWorldDir2D.y); while (currentDir < desiredDir - gf_PI) currentDir += 2.0f * gf_PI; while (currentDir > desiredDir + gf_PI) currentDir -= 2.0f * gf_PI; // ---------------------------- Yaw ---------------------------- Ang3 dirDiff(0.0f, 0.0f, desiredDir - currentDir); dirDiff.RangePI(); float absDiff = fabsf(dirDiff.z); float rotSpeed = (float)__fsel(dirDiff.z, m_yawPerRoll, -m_yawPerRoll); m_actionYaw = m_actionYaw + deltaTime * (rotSpeed - m_actionYaw); float temp = fabsf(m_actionYaw); float multiplier = ((absDiff / (temp + 0.001f)) + 1.0f) * 0.5f; m_actionYaw *= (float)__fsel(absDiff - temp, 1.0f, multiplier); // ---------------------------- Yaw ------------------------------ m_CurrentVel = desiredMoveDir * m_CurrentSpeed; // ---------------------------- Pitch ---------------------------- if (m_CurrentVel.GetLengthSquared2D() > 0.1f) { CalculatePitch(worldAngles, desiredMoveDir, currentSpeed2d, desiredSpeed, deltaTime); } else { Quat rot; rot.SetRotationVDir(desiredLookDir, 0.0f); float desiredXRot = Ang3::GetAnglesXYZ(rot).x + m_steeringDamage.x; m_actionPitch = worldAngles.x + (desiredXRot - worldAngles.x) * deltaTime/* * 10.0f*/; Limit(m_actionPitch, -m_maxPitchAngle * 2.0f, m_maxPitchAngle * 2.0f); } // ---------------------------- Roll ---------------------------- float rollSpeed = GetRollSpeed(); rollSpeed *= deltaTime; rollSpeed = (float)__fsel(absDiff - rollSpeed, rollSpeed, absDiff); float roll =(float) __fsel(dirDiff.z, -rollSpeed, rollSpeed); float speedPerUnit = 1.5f; float desiredRollSpeed = absDiff * speedPerUnit * (float)__fsel(dirDiff.z, 1.0f, -1.0f); desiredRollSpeed = -m_actionYaw * 2.5f; desiredRollSpeed += m_steeringDamage.y; m_actionRoll = m_actionRoll + deltaTime * (desiredRollSpeed - m_actionRoll); Limit(m_actionRoll, -m_maxRollAngle + m_steeringDamage.y, m_maxRollAngle - m_steeringDamage.y); m_actionRoll *= m_rollDamping; // ---------------------------- Roll ---------------------------- // ---------------------------- Convert and apply ---------------------------- Ang3 angles(m_actionPitch, m_actionRoll, worldAngles.z + deltaTime * m_actionYaw); pe_params_pos paramPos; paramPos.q.SetRotationXYZ(angles); paramPos.q.Normalize(); IPhysicalEntity * pPhysicalEntity = GetPhysics(); pPhysicalEntity->SetParams(¶mPos, 1); pe_action_set_velocity vel; vel.v = m_CurrentVel + m_netPosAdjust; pPhysicalEntity->Action(&vel, 1); // ---------------------------- Convert and apply ---------------------------- m_rpmScale = max(0.2f, cry_fabsf(m_CurrentSpeed / m_maxSpeed)); }
void HitSpinnerWall( Spinner* sp, Plane &plane, Ball* b ) { Vector3 wall = plane.points[2] - plane.points[0]; Vector3 ballFromWall = b->Position() - plane.points[0]; float relativePosition = plane.normal * ballFromWall; float relativeDirection = plane.normal * b->Velocity(); if ((relativePosition > 0 && relativeDirection < 0) || (relativePosition < 0 && relativeDirection > 0)) { float theta, c, s; theta = acos((wall * Vector3(1,0,0))/wall.getLength()); if (b->position.x<0) { theta *= -1; } c = cos(theta), s = sin(theta); Vector3 position2 = Vector3(); Vector3 velocityRot = Vector3(); position2 = Vector3(c * ballFromWall.x + s * ballFromWall.y, c * ballFromWall.y - s * ballFromWall.x, 0); velocityRot = Vector3( c * b->Velocity().x + s * b->Velocity().y, c * b->Velocity().y - s * b->Velocity().x, 0); // Set the ball outside the wall, reverse velocity toward wall Vector3 scaledNormal = b->radius * Vector3(0,-1,0); position2.y = scaledNormal.y; velocityRot.y *= -1; // Rotate back ballFromWall = Vector3(c * position2.x - s * position2.y,c * position2.y + s * position2.x,0); b->Velocity(Vector3(c * velocityRot.x - s * velocityRot.y,c * velocityRot.y + s * velocityRot.x,0)); Point3 newPos = plane.points[0] + ballFromWall; b->position = newPos; //float bEnergy = .5 * b->Mass() * b->Velocity() * b->Velocity(); Vector3 momenteum = b->Mass() * b->Velocity(); Point3 closest = plane.ClosestPointToPoint(b->Position()); //Vector3 tempV = diff; //tempV.normalize(); //Vector3 proj = (closest*tempV)*tempV; //float temp = proj.getLength(); //// check if in the bounding box //if(diff.getLength() > temp + b->radius) //{ // continue; //} //// then it needs to go, "Hey!". ////find the force applied to the bar // ft = dp/dt = m*dv/dt ////find perpendicular momentum //Vector3 totalForce = bEnergy * b->Velocity(); //// t = r X F //Vector3 torque = diff.cross(totalForce); Vector3 radius = Point3(closest.x,closest.y,closest.z) - sp->Position(); Vector3 torque = radius.cross(momenteum); Vector3 dL = .05 * torque; //// find the rotation matrix and the inverse of the rotation matrix Matrix33 myRotation = Matrix33(sp->Rotation()); Matrix33 myRotationT = myRotation.Transpose(); sp->MyL(sp->MyL() + dL); ////w(i+1) = q(i+1) * I(-1) * qT(i+1) * L(i+1) sp->AngularVel(myRotation * *sp->Tensor()->inverse() * myRotationT * sp->MyL()); ////q(n+1) = q(n) + h(.5 * w(n)*q(n)) sp->Rotation(sp->Rotation() + .05f*(.5 * Quaternion(sp->AngularVel()) * sp->Rotation())); Quaternion q = sp->Rotation(); q.normalize(); sp->Rotation(q); } }
void GhostBody::SetGeomRotation(Matrix33 const & matrix) { ASSERT(_body_handle); dBodySetRotation(_body_handle, reinterpret_cast<Scalar const *>(matrix.GetArray())); }
void OpenGLContext::setIdentity() { Matrix33* active = getActiveMatrix(); active->setIdentity(); loadGLMatrix(); }
void CPlayerRotation::ClampAngles() { { //cap up/down looking float minAngle,maxAngle; GetStanceAngleLimits(minAngle,maxAngle); float currentViewPitch=GetLocalPitch(); float newPitch = currentViewPitch + m_deltaAngles.x; if(newPitch < minAngle) newPitch = minAngle; else if(newPitch > maxAngle) newPitch = maxAngle; m_deltaAngles.x = newPitch - currentViewPitch; } { //further limit the view if necessary float limitV = m_params.vLimitRangeV; float limitH = m_params.vLimitRangeH; Vec3 limitDir = m_params.vLimitDir; float limitVUp = m_params.vLimitRangeVUp; float limitVDown = m_params.vLimitRangeVDown; if(m_player.m_stats.isFrozen.Value()) { float clampMin = g_pGameCVars->cl_frozenAngleMin; float clampMax = g_pGameCVars->cl_frozenAngleMax; float frozenLimit = DEG2RAD(clampMin + (clampMax-clampMin)*(1.f-m_player.GetFrozenAmount(true))); if(limitV == 0 || limitV>frozenLimit) limitV = frozenLimit; if(limitH == 0 || limitH>frozenLimit) limitH = frozenLimit; if(g_pGameCVars->cl_debugFreezeShake) { static float color[] = {1,1,1,1}; gEnv->pRenderer->Draw2dLabel(100,200,1.5,color,false,"limit: %f", RAD2DEG(frozenLimit)); } } if(m_player.m_stats.isOnLadder) { limitDir = -m_player.m_stats.ladderOrientation; limitH = DEG2RAD(40.0f); } if((limitH+limitV+limitVUp+limitVDown) && limitDir.len2()>0.1f) { //A matrix is built around the view limit, and then the player view angles are checked with it. //Later, if necessary the upVector could be made customizable. Vec3 forward(limitDir); Vec3 up(m_baseQuat.GetColumn2()); Vec3 right(-(up % forward)); right.Normalize(); Matrix33 limitMtx; limitMtx.SetFromVectors(right,forward,right%forward); //gEnv->pRenderer->GetIRenderAuxGeom()->DrawLine(m_player.GetEntity()->GetWorldPos(), ColorB(0,0,255,255), m_player.GetEntity()->GetWorldPos() + limitMtx.GetColumn(0), ColorB(0,0,255,255)); //gEnv->pRenderer->GetIRenderAuxGeom()->DrawLine(m_player.GetEntity()->GetWorldPos(), ColorB(0,255,0,255), m_player.GetEntity()->GetWorldPos() + limitMtx.GetColumn(1), ColorB(0,255,0,255)); //gEnv->pRenderer->GetIRenderAuxGeom()->DrawLine(m_player.GetEntity()->GetWorldPos(), ColorB(255,0,0,255), m_player.GetEntity()->GetWorldPos() + limitMtx.GetColumn(2), ColorB(255,0,0,255)); limitMtx.Invert(); Vec3 localDir(limitMtx * m_viewQuat.GetColumn1()); // Vec3 localDir(limitMtx * m_player.GetEntity()->GetWorldRotation().GetColumn1()); Ang3 limit; if(limitV) { limit.x = asinf(localDir.z) + m_deltaAngles.x; float deltaX(limitV - fabs(limit.x)); if(deltaX < 0.0f) m_deltaAngles.x += deltaX*(limit.x>0.0f?1.0f:-1.0f); } if(limitVUp || limitVDown) { limit.x = asinf(localDir.z) + m_deltaAngles.x; if(limit.x>=limitVUp && limitVUp!=0) { float deltaXUp(limitVUp - limit.x); m_deltaAngles.x += deltaXUp; } if(limit.x<=limitVDown && limitVDown!=0) { float deltaXDown(limitVDown - limit.x); m_deltaAngles.x += deltaXDown; } } if(limitH) { limit.z = cry_atan2f(-localDir.x,localDir.y) + m_deltaAngles.z; float deltaZ(limitH - fabs(limit.z)); if(deltaZ < 0.0f) m_deltaAngles.z += deltaZ*(limit.z>0.0f?1.0f:-1.0f); } } } }
int main(int arg, char* argc[]) { DetermineCameraPositionParser parser(helpMsg); try{ parser.parse(arg,argc); MarkerType markerType; vector<Point2d> markers; string dataFolder = parser.get<string>("dataFolder"); int camID = parser.get<int>("camID"); int camW = parser.get<int>("camWidth"); int camH = parser.get<int>("camHeight"); string markerName = dataFolder + "/markers/" + parser.get<string>("markerName") + ".txt"; markerType = MarkerType(markerName); string distanceFile = dataFolder + "/distances/" + parser.get<string>("distanceMatrix") + ".txt"; MatrixDD distances = loadDistanceMatrix(distanceFile); Mat src; string cameraFileName = dataFolder + "/camera/" + parser.get<string>("cameraName") + ".txt"; Camera cam = loadCameraFromFile( cameraFileName ); VideoCapture cap(camID); cap.set(CV_CAP_PROP_FRAME_WIDTH,cam.getResWidth()); cap.set(CV_CAP_PROP_FRAME_HEIGHT,cam.getResHeight()); if (!cap.isOpened()) return false; Matrix33 planeR = Matrix33::Identity(); Vector3 planeT; while (1) { cap >> src; // Track markers trackMarker(src, markerType, markers); // sort markers in circular patern reorganizePoints(markers); // print markers in picture for (size_t i = 0; i < markers.size(); i++) { circle(src, markers[i], 1, Scalar(255, 0, 0), 2); stringstream ss; ss << i; putText(src, ss.str(), markers[i], FONT_HERSHEY_PLAIN, 1.5, Scalar(255, 0, 0)); } if (markers.size() == distances.rows() ) { vector<Vector3> outPoints; double optiError; vector<Real> x(distances.rows(),2000); optiError = findPointPositions(distances, markers, cam, outPoints, x); findPlaneCoordinateFrame( cam, outPoints, planeR, planeT ); cout << "Camera is: " << (planeR.transpose()*(cam.getT() - planeT))[2] << "mm above ground." << endl; cout << "Marker distances are: "; for(Real y : x){ cout << y << " "; } cout << endl << endl; }else{ cout << "Detected " << markers.size() << " ground markers but there should be " << distances.rows() << " markers in the picture!" << endl; } imshow("determineCameraPosition", src); if (waitKey(1) >= 0) break; } }catch( std::exception const & exc ){ cerr << exc.what() << endl; return 0; } return 0; }
void CCameraInputHelper::UpdateCameraInput(Ang3 &deltaRotation, const float frameTimeClamped, const float frameTimeNormalised) { // Use new control scheme, // or, if we're just entering/leaving it then adjust camera/character orientations as appropriate. //--- Translate left stick into a deltaRotation.z (and a move direction) Vec3 vcStick=Vec3(m_pHeroInput->m_moveStickLR, m_pHeroInput->m_moveStickUD,0); // Stick vector in view space // -m_moveStickLR removes flicker, but inverses direction.. static Vec3 vcStickPrev=Vec3(0,1,0); float stickMag=vcStick.len(); if(stickMag>0.001f) vcStick*=1/stickMag; else vcStick=vcStickPrev; // should hold previous value/direction unless we're in transition. vcStickPrev=vcStick; Matrix33 camViewMtx = m_pHero->m_camViewMtxFinal; //in flight mode the actual camera matrix changes //the mtxFinal itself stays untouched, so that the camera can move back after the flight if(CCameraFlight::GetInstance()->IsCameraFlightActive()) { Vec3 vCamDir = CCameraFlight::GetInstance()->GetLookingDirection(); camViewMtx.SetRotationVDir(vCamDir, 0.0f); } const Matrix34& entityWorldTM = m_pHero->GetEntity()->GetWorldTM(); Vec3 vwEntPos = entityWorldTM.GetTranslation(); Vec3 vwDrawFrom = vwEntPos+Vec3(0,0,0.8f); Vec3 vwEntityFront = entityWorldTM.GetColumn1(); Vec3 vwEntityRight = entityWorldTM.GetColumn0(); Vec3 vCamRef = camViewMtx*Vec3(0,1,0); Vec3 vwStick = camViewMtx*vcStick; // Stick vector in world space Vec3 vwfStick = Vec3FlattenXY(vwStick); Vec3 vwfEntityFront=Vec3FlattenXY(vwEntityFront); Vec3 vwfEntityRight=Vec3FlattenXY(vwEntityRight); float dotRight=vwfStick.Dot(vwfEntityRight); float dotFront=vwfStick.Dot(vwfEntityFront); dotFront = clamp(dotFront,-1.0f,1.0f); bool enteredNavFromCombat = false; //(m_bNavCombatModeChanged && !bIsCombatMode); //--- Just moved into nav mode from combat if(enteredNavFromCombat) { // so set directions appropriately. float dotFront2=(vCamRef).Dot(Vec3(0,1,0)); float dotRight2=(camViewMtx*Vec3(1,0,0)).Dot(Vec3(0,1,0)); float yaw = 0.0f; dotFront2 = clamp(dotFront2,-1.0f,1.0f); if(dotRight2>=0) { yaw=-cry_acosf(dotFront2); } else { yaw=cry_acosf(dotFront2); } m_fYaw = yaw; } //this is the actual entity rotation (yaw movement) CCameraOverrides *pCamOverride = g_pGame->GetCameraManager()->GetCamOverrides(); int eCamOverrideMode = pCamOverride->GetCameraOverride(); //the ECO_LOWCOVER fading is a hack, causing problems when inputs are changed during the fade //in combat mode all inputs have to be passed through to synchronize the camera direction in other modes if((stickMag>0.01f && eCamOverrideMode != ECO_LOWCOVER)) { if(dotRight>=0) { deltaRotation.z=-cry_acosf(dotFront); } else { deltaRotation.z=cry_acosf(dotFront); } } else deltaRotation.z=0; float maxDeltaRot = 7.85f; // safety value //g_pGameCVars->cl_nav_SprintMaxTurnRate; Limit( maxDeltaRot, 0.f, gf_PI * 10.f ); // limit to 3600 degrees per second, i.e. 'pretty fast' maxDeltaRot *= frameTimeClamped; // In Nav mode movement is always in the characters forward direction. UpdatePitchYawDriver(stickMag, frameTimeNormalised); m_bNavCombatModeChanged=false; }