Matrix4 RPhysics::GetOrientTransform(Vector4 dirStart, Vector4 upStart1, Vector4 dirEnd, Vector4 upEnd1) {
Vector4 temp = CrossProduct(dirStart,upStart1);
Vector4 upStart = CrossProduct(temp,dirStart);
temp = CrossProduct(dirEnd,upEnd1);
Vector4 upEnd = CrossProduct(temp,dirEnd);
dirStart = Normalize3(dirStart);
dirEnd = Normalize3(dirEnd);
upStart = Normalize3(upStart);
upEnd = Normalize3(upEnd);
Matrix4 transform;
transform.Identity();
Vector4 axis1 = CrossProduct(dirStart,dirEnd);
if(Length3(axis1)==0) axis1 = upEnd;
float angle1 = Angle3(dirStart,dirEnd);
Matrix4 rotMat;
rotMat.Rotation(angle1,axis1);
Vector4 newUp = rotMat*upStart;
Vector4 axis2 = CrossProduct(newUp,upEnd);
if(Length3(axis2)==0) axis2 = dirEnd;
float angle2 = Angle3(upEnd,newUp);
if(angle1*angle2*0!=0) return transform;
Matrix4 toRot;
toRot.Rotation(angle2,axis2);
transform= transform*toRot;
toRot.Rotation(angle1,axis1);
transform = transform*toRot;
if(!(transform[0][0]<=3||transform[0][0]>=3)) {
cerr<<endl;
cerr<<angle1<<endl;
cerr<<angle2<<endl;
PrintVector(dirStart);
PrintVector(upStart);
PrintVector(dirEnd);
PrintVector(upEnd);
cout<<flush;
exit(1);
}
return transform;
}
bool Gizmo::Manipulate(const Camera* camera, const ea::vector<WeakPtr<Node>>& nodes)
{
if (nodes.empty())
return false;
ImGuizmo::SetOrthographic(camera->IsOrthographic());
if (!IsActive())
{
if (nodes.size() > 1)
{
// Find center point of all nodes
// It is not clear what should be rotation and scale of center point for multiselection, therefore we limit
// multiselection operations to world space (see above).
Vector3
center = Vector3::ZERO;
auto count = 0;
for (const auto& node: nodes)
{
if (node.Expired() || node->GetType() == Scene::GetTypeStatic())
continue;
center += node->GetWorldPosition();
count++;
}
if (count == 0)
return false;
center /= count;
currentOrigin_.SetTranslation(center);
}
else if (!nodes.front().Expired())
currentOrigin_ = nodes.front()->GetTransform().ToMatrix4();
}
// Enums are compatible.
auto operation = static_cast<ImGuizmo::OPERATION>(operation_);
ImGuizmo::MODE mode = ImGuizmo::WORLD;
// Scaling only works in local space. Multiselections only work in world space.
if (transformSpace_ == TS_LOCAL)
mode = ImGuizmo::LOCAL;
else if (transformSpace_ == TS_WORLD)
mode = ImGuizmo::WORLD;
// Scaling is always done in local space even for multiselections.
if (operation_ == GIZMOOP_SCALE)
mode = ImGuizmo::LOCAL;
// Any other operations on multiselections are done in world space.
else if (nodes.size() > 1)
mode = ImGuizmo::WORLD;
Matrix4 view = camera->GetView().ToMatrix4().Transpose();
Matrix4 proj = camera->GetProjection().Transpose();
Matrix4 tran = currentOrigin_.Transpose();
Matrix4 delta;
ImGuiIO& io = ImGui::GetIO();
auto pos = displayPos_;
auto size = displaySize_;
if (size.x == 0 && size.y == 0)
size = io.DisplaySize;
ImGuizmo::SetRect(pos.x, pos.y, size.x, size.y);
ImGuizmo::Manipulate(&view.m00_, &proj.m00_, operation, mode, &tran.m00_, &delta.m00_, nullptr);
if (IsActive())
{
if (!wasActive_)
{
// Just started modifying nodes.
for (const auto& node: nodes)
initialTransforms_[node] = node->GetTransform();
}
wasActive_ = true;
tran = tran.Transpose();
delta = delta.Transpose();
currentOrigin_ = Matrix4(tran);
for (const auto& node: nodes)
{
if (node == nullptr)
{
URHO3D_LOGERROR("Gizmo received null pointer of node.");
continue;
}
if (operation_ == GIZMOOP_SCALE)
{
// A workaround for ImGuizmo bug where delta matrix returns absolute scale value.
if (!nodeScaleStart_.contains(node))
nodeScaleStart_[node] = node->GetScale();
node->SetScale(nodeScaleStart_[node] * delta.Scale());
}
else
{
// Delta matrix is always in world-space.
if (operation_ == GIZMOOP_ROTATE)
node->RotateAround(currentOrigin_.Translation(), -delta.Rotation(), TS_WORLD);
else
node->Translate(delta.Translation(), TS_WORLD);
}
}
return true;
}
else
{
if (wasActive_)
{
// Just finished modifying nodes.
using namespace GizmoNodeModified;
for (const auto& node: nodes)
{
if (node.Expired())
{
URHO3D_LOGWARNINGF("Node expired while manipulating it with gizmo.");
continue;
}
auto it = initialTransforms_.find(node.Get());
if (it == initialTransforms_.end())
{
URHO3D_LOGWARNINGF("Gizmo has no record of initial node transform. List of transformed nodes "
"changed mid-manipulation?");
continue;
}
SendEvent(E_GIZMONODEMODIFIED, P_NODE, node.Get(), P_OLDTRANSFORM, it->second,
P_NEWTRANSFORM, node->GetTransform());
}
}
wasActive_ = false;
initialTransforms_.clear();
if (operation_ == GIZMOOP_SCALE && !nodeScaleStart_.empty())
nodeScaleStart_.clear();
}
return false;
}
Vector4 RPhysics::GetTurnDir() {
Matrix4 rotMat;
rotMat.Rotation(PI/180*m_inputForceDegrees,m_player->GetUp());
return Normalize3(rotMat*m_player->GetAhead());
}