Quaternion Quaternion::operator/(const Quaternion & rhs)const
{
return *this*rhs.i();
}
void DumpBSpline(NiBSplineCompTransformInterpolatorRef spline, string target)
{
float start = spline->GetStartTime();
float stop = spline->GetStopTime();
int nframes = int((stop-start) / FramesIncrement);
Ref<NiBSplineData > data = spline->GetSplineData();
Ref<NiBSplineBasisData > basis = spline->GetBasisData();
int nctrl = basis->GetNumControlPoints();
cout.setf(ios::fixed, ios::floatfield);
cout << setprecision(3); // << setiosflags(ios_base::showpos);
if (dumpControl)
{
vector<short > control = data->GetShortControlPoints();
cout << "Control data for " << target << endl
<< "Control Pt: " << control.size() << endl;
int j=0;
cout << "0" << '\t';
for (int i=0, n=control.size(); i<n; ++i, ++j) {
//cout << float(control[i]) / float (32767) << '\t';
cout << i << '\t' << control[i] << endl;
}
cout << endl;
return;
}
{
vector<Vector3> control = spline->GetTranslateControlData();
if (!control.empty()) {
float mult = spline->GetTranslateMultiplier();
float bias = spline->GetTranslateBias();
cout << "Translation data for " << target << endl
<< "Control Pt: " << control.size() << endl
<< "Mult: " << mult << endl
<< "Bias: " << bias << endl
<< endl ;
for (int i=0, n=control.size(); i<n; ++i){
Vector3 xyz = control[i];
cout << i << "\t[" << xyz.x << ",\t" << xyz.y << ",\t" << xyz.z << "]" << endl;
}
cout << endl;
int npoints = control.size() * 2 + 1;
vector< Key<Vector3> > keys = spline->SampleTranslateKeys(npoints, 3);
for (int i=0, n=keys.size(); i<n; ++i){
Vector3 xyz = keys[i].data;
cout << i << "\t[" << xyz.x << ",\t" << xyz.y << ",\t" << xyz.z << "]" << endl;
}
cout << endl;
}
}
{
vector<Quaternion> control = spline->GetQuatRotateControlData();
if (!control.empty()) {
float mult = spline->GetRotationMultiplier();
float bias = spline->GetRotationBias();
cout << "Quaternion rotation data for " << target << endl
<< "Control Pt: " << control.size() << endl
<< "Mult: " << mult << endl
<< "Bias: " << bias << endl
<< endl ;
for (int i=0, n=control.size(), j=1; i<n; ++i, ++j){
Quaternion q = control[i];
Float3 ypr = q.AsEulerYawPitchRoll();
cout << i << "\t<" << q.w << ",\t" << q.x << ",\t" << q.y << ",\t" << q.z << ">"
<< "\t[" << TODEG(ypr[0]) << ",\t" << TODEG(ypr[1]) << ",\t" << TODEG(ypr[2]) << "]"
<< endl;
if (j==nctrl)
{
cout << endl;
j = 0;
}
}
cout << endl;
int npoints = control.size() * 2 + 1;
vector< Key<Quaternion> > keys = spline->SampleQuatRotateKeys(npoints, 3);
for (int i=0, n=keys.size(); i<n; ++i){
Quaternion q = keys[i].data;
Float3 ypr = q.AsEulerYawPitchRoll();
cout << i << "\t<" << q.w << ",\t" << q.x << ",\t" << q.y << ",\t" << q.z << ">"
<< "\t[" << TODEG(ypr[0]) << ",\t" << TODEG(ypr[1]) << ",\t" << TODEG(ypr[2]) << "]"
<< endl;
}
cout << endl;
}
}
{
vector<float> control = spline->GetScaleControlData();
if (!control.empty()) {
float mult = spline->GetScaleMultiplier();
float bias = spline->GetScaleBias();
cout << "Scale data for " << target << endl
<< "Control Pt: " << control.size() << endl
<< "Mult: " << mult << endl
<< "Bias: " << bias << endl
<< endl ;
for (int i=0, n=control.size(); i<n; ++i){
float s = control[i];
cout << i << "\t" << s << endl;
}
cout << endl;
int npoints = control.size() * 2 + 1;
vector< Key<float> > keys = spline->SampleScaleKeys(npoints, 3);
for (int i=0, n=keys.size(); i<n; ++i){
float s = keys[i].data;
cout << i << "\t" << s << endl;
}
cout << endl;
}
}
//extern void bspline(int n, int t, int l, float *control, float *output, int num_output);
//
//Float4 *p = new Float4[nctrl];
//for (int i=0, j=0;i<nctrl; ++i) {
// p[i][0] = float(points[j++]) / float (32767);
// p[i][1] = float(points[j++]) / float (32767);
// p[i][2] = float(points[j++]) / float (32767);
// p[i][3] = float(points[j++]) / float (32767);
// float w = float(p[i][0]) * mult + bias;
// float x = float(p[i][1]) * mult + bias;
// float y = float(p[i][2]) * mult + bias;
// float z = float(p[i][3]) * mult + bias;
// Quaternion q (w,z,x,y);
// Float3 ypr = q.AsEulerYawPitchRoll();
// cout << i
// << "\t" << w
// << "\t" << x
// << "\t" << y
// << "\t" << z
// << "\t" << TODEG(ypr[0])
// << "\t" << TODEG(ypr[1])
// << "\t" << TODEG(ypr[2])
// << endl;
//}
//cout << endl;
//int res = (nframes+1)*2+1;
//Float4 *out = new Float4[res];
//bspline(nctrl-1, 4, 4, &p[0][0], &out[0][0], res);
//for (int i = 0; i < res; ++i)
//{
// float fT = ((float)i)/((float)res-1) * (stop-start) * FramesPerSecond;
// float w = float(out[i][0]) * mult + bias;
// float x = float(out[i][1]) * mult + bias;
// float y = float(out[i][2]) * mult + bias;
// float z = float(out[i][3]) * mult + bias;
// Quaternion q (w,z,x,y);
// Float3 ypr = q.AsEulerYawPitchRoll();
// cout << i
// << "\t" << fT
// << "\t" << w
// << "\t" << x
// << "\t" << y
// << "\t" << z
// << "\t" << TODEG(ypr[0])
// << "\t" << TODEG(ypr[1])
// << "\t" << TODEG(ypr[2])
// << endl;
//}
}
bool IK::Solve(Vector3 targetPos)
{
//http://mrl.nyu.edu/~perlin/gdc/ik/ik.java.html
//Get nodes
Node* hip = effector_->GetParent()->GetParent();
Node* knee = effector_->GetParent();
// Get current world position for the 3 joints of the IK chain
Vector3 hipPos = hip->GetWorldPosition(); // Thigh pos (hip joint)
Vector3 kneePos = knee->GetWorldPosition(); // Calf pos (knee joint)
Vector3 effectorPos = effector_->GetWorldPosition(); // Foot pos (ankle joint)
// Pre IK Direction vectors
Vector3 thighDir = kneePos - hipPos; // Thigh direction
Vector3 calfDir = effectorPos - kneePos; // Calf direction
// Vectors lengths
float A = Vector3(thighDir).Length();//length of hip to knee
float B = Vector3(calfDir).Length();//length of knee to foot
Vector3 P = hip->WorldToLocal(targetPos);//target at origin
Vector3 D = hip->WorldToLocal(kneePos);//pre solve knee at origin
//float limbLength = length1 + length2;
//float lengthH = targetDir.Length();
//PERLINS STUFF
//bool test = Perlin(A,B,C,D);
//GetSubsystem<DebugHud>()->SetAppStats("ik:", String(test) );
//------
Vector3 R;
DefineM(P,D);
R = Rot(Minv,P);
//FIND D
float c = R.Length();
float d = Max(0.0f, Min(A, (c + (A*A-B*B)/c) / 2.0f));//FindD(A,B,R.Length());
//FIND E
float e = sqrtf(A*A-d*d);//FindE(A,d);
Vector3 S = Vector3(d,e,0.0f);
Vector3 Q = Rot(Mfwd,S);
//Convert Q back to world space
Vector3 worldQ = effector_->GetParent()->GetParent()->LocalToWorld(Q);
//Get angles
Vector3 tdn = thighDir.Normalized();
Vector3 ntdn = Vector3(worldQ-hipPos).Normalized();
Vector3 cdn = calfDir.Normalized();
Vector3 ncdn = Vector3(targetPos-worldQ).Normalized();
//Vector3 hipAxis = tdn.CrossProduct(ntdn);
//float hipAngle = tdn.Angle(ntdn);
//Vector3 kneeAxis = cdn.CrossProduct(ncdn);
//float kneeAngle = cdn.Angle(ncdn);
//GetSubsystem<DebugHud>()->SetAppStats("ik:", String(hipAngle)+":"+String(kneeAngle) );
//knee->SetWorldRotation(knee->GetWorldRotation() * Quaternion(kneeAngle,kneeAxis) );
//hip->SetWorldRotation(hip->GetWorldRotation() * Quaternion(hipAngle,hipAxis) );
//do top level first, then get new angle for lower level, since it might mangle it
bool success = d > 0.0f && d < A;
if(success)
{
Quaternion hipRot = Quaternion(tdn,ntdn);
hip->Rotate(hipRot,TS_WORLD );
knee->Rotate(Quaternion(cdn,ncdn)*hipRot.Inverse(),TS_WORLD );
}
if(drawDebug_)
{
DebugRenderer* dbg = effector_->GetScene()->GetComponent<DebugRenderer>();
/*dbg->AddLine(hipPos,hipPos+tdn,Color(0.0f,1.0f,0.0f),false);
dbg->AddLine(hipPos,hipPos+ntdn,Color(0.0f,0.0f,1.0f),false);
dbg->AddLine(kneePos,kneePos+cdn,Color(0.0f,1.0f,0.0f),false);
dbg->AddLine(kneePos,kneePos+ncdn,Color(0.0f,0.0f,1.0f),false);
dbg->AddSphere(Sphere(effectorPos,0.2f),Color(0.0f,1.0f,0.0f),false);
dbg->AddSphere(Sphere(targetPos,0.2f),Color(0.0f,0.0f,1.0f),false);*/
//at origin
/*dbg->AddSphere(Sphere(Vector3(),0.2f),Color(0.0f,0.0f,0.0f),false);//origin
dbg->AddSphere(Sphere(D,0.2f),Color(0.1f,0.0f,0.0f),false);//old elbow
dbg->AddSphere(Sphere(P,0.2f),Color(0.0f,1.0f,0.0f),false);//target
dbg->AddLine(Vector3(),P,Color(0.1f,0.1f,0.1f),false);
//show solve at origin
dbg->AddSphere(Sphere(Q,0.2f),Color(1.0f,0.0f,0.0f),false);
dbg->AddLine(Vector3(),Q,Color(1.0f,0.0f,0.0f),false);
dbg->AddLine(Q,P,Color(1.0f,0.0f,0.0f),false);*/
//show solve at position
dbg->AddSphere(Sphere(worldQ,0.2f),Color(1.0f,0.0f,0.0f),false);
dbg->AddSphere(Sphere(targetPos,0.2f),Color(0.0f,1.0f,0.0f),false);
dbg->AddLine(hipPos,worldQ,Color(1.0f,0.0f,0.0f),false);
dbg->AddLine(worldQ,targetPos,Color(1.0f,0.0f,0.0f),false);
}
return success;
}
/*!
Moves the element from its current pose to the new pose specified by \a tr.
This motion is performed in several steps such that the translation
between each step does not exceed \a translStepSize and the angle of
rotation does not exceed \a rotStepSize (expressed in radians). The
intermediate poses are determined using linear interpolation for the
translation and spherical linear interpolation for the rotation. If
a collision is encountered during the motion, the point of first contact
is determined and the element is left in that position. This function
returns false if a collision was encountered (or contacts prevented the motion)
or true if no collisions were encountered and the move was completed.
*/
bool
WorldElement::moveTo(transf &newTran, double translStepSize, double rotStepSize)
{
bool moveFinished = false;
transf origTran, nextTran, motion;
Quaternion nextRotation;
vec3 nextTranslation;
double percentComplete, moveIncrement, translationLength;
double angle;
vec3 axis;
bool success;
CollisionReport contactReport;
//DBGP("moveTo called");
origTran = getTran();
/*
std::cout << "WorldElement origTran: " << origTran.translation().x() << " " <<
origTran.translation().y() << " " <<
origTran.translation().z() << " " <<
origTran.rotation().w << " " <<
origTran.rotation().x << " " <<
origTran.rotation().y << " " <<
origTran.rotation().z << " " << "\n";
*/
//calculate the difference
translationLength = (newTran.translation() - origTran.translation()).len();
nextRotation = newTran.rotation() * origTran.rotation().inverse();
nextRotation.ToAngleAxis(angle, axis);
moveIncrement = 1.0;
if (translationLength != 0.0) {
if (translStepSize == ONE_STEP) {
moveIncrement = 1.0;
}
else {
moveIncrement = MIN(moveIncrement, translStepSize / translationLength);
}
}
if (angle != 0.0) {
if (rotStepSize == ONE_STEP) {
moveIncrement = MIN(moveIncrement, 1.0);
}
else {
moveIncrement = MIN(moveIncrement, rotStepSize / angle);
}
}
// check contacts
nextTranslation = (1.0 - moveIncrement) * origTran.translation() + moveIncrement * newTran.translation();
nextRotation = Quaternion::Slerp(moveIncrement, origTran.rotation(), newTran.rotation());
nextTran = transf(nextRotation, nextTranslation);
motion = nextTran * getTran().inverse();
if (contactsPreventMotion(motion)) {
DBGP("contacts prevent motion")
return false;
}
percentComplete = 0.0;
while (!moveFinished) {
percentComplete += moveIncrement;
if (percentComplete >= 1.0) {
percentComplete = 1.0;
moveFinished = true;
}
nextTranslation = (1.0 - percentComplete) * origTran.translation() + percentComplete * newTran.translation();
nextRotation = Quaternion::Slerp(percentComplete, origTran.rotation(), newTran.rotation());
nextTran = transf(nextRotation, nextTranslation);
/*
std::cout << "moveTo NextTran: " << nextTran.translation().x() << " " <<
nextTran.translation().y() << " " <<
nextTran.translation().z() << " " <<
nextTran.rotation().w << " " <<
nextTran.rotation().x << " " <<
nextTran.rotation().y << " " <<
nextTran.rotation().z << " " << "\n";
*/
success = jumpTo(nextTran, &contactReport);
if (!success || contactReport.size() != 0) {
moveFinished = true;
}
}
if (!success) {
DBGA("JumpTo error, stopping execution. Object " << myName.latin1() << " in thread "
<< getWorld()->getCollisionInterface()->getThreadId());
} else {
myWorld->findContacts(contactReport);
}
if (contactReport.size() != 0) { return false; }
return true;
}
void DefaultCameraManager::setOrientation(Quaternion ori)
{
mCameraPosNode->setOrientation(ori.toOgre());
}
void DefaultCameraManager::setPosition(Vector3 pos,Quaternion dir,Vector3 offset)
{
mCameraPosNode->setPosition(pos+dir.toOgre()*offset);
}