Пример #1
0
void quatRotate(Quaternion *q, Quaternion *v, Quaternion *result) {

    if(q == NULL || v == NULL || result == NULL) { return; }

    Quaternion temp;
    quatConj(q, &temp);
    quatMult(q, v, result);
    quatMult(result, &temp, result);

}
Пример #2
0
void rateProcess(void) {

    Quaternion current_ref;

    if(!is_ready || !is_running) { return; }
    
    rgltrGetQuatRef(&current_ref);    

    // q_body*q_current*q_global    
    quatMult(&global_displacement, &current_ref, &current_ref);
    quatMult(&current_ref, &body_displacement, &current_ref);

    quatNormalize(&current_ref);

    rgltrSetQuatRef(&current_ref);

}
Пример #3
0
// qtarget = qcurr*qdisp
// qcurr'*qtarget = qdisp
void slewProcess(Quaternion *input, Quaternion *output) {

    Quaternion conjugate, displacement;
    bams32_t input_angle, limited_angle;
    float sin_input, sin_limited, scale;

    if(!is_ready || !is_running || max_angular_displacement == 0) {
        quatCopy(output, input);
        return;
    }

    // Calculate displacement
    quatConj(&prev_ref, &conjugate);
    quatMult(&conjugate, input, &displacement);

    // Calculate displacement magnitude
    input_angle = bams16ToBams32(bams16Acos(displacement.w)*2);
    if(input_angle == 0) { // Check for no displacement case
        quatCopy(output, input);
        return;
    }
    sin_input = bams32Sin(input_angle/2);

    // Apply displacement limits
    if(input_angle > max_angular_displacement) {
        limited_angle = max_angular_displacement;
    } else if(input_angle < -max_angular_displacement) {
        limited_angle = -max_angular_displacement;
    } else {
        limited_angle = input_angle;
    }

    sin_limited = bams32SinFine(limited_angle/2);
    scale = sin_limited/sin_input;

    displacement.w = bams32CosFine(limited_angle/2);
    displacement.x = displacement.x*scale;
    displacement.y = displacement.y*scale;
    displacement.z = displacement.z*scale;

    // Apply limited displacement
    quatMult(&prev_ref, &displacement, output);
    quatNormalize(output);
    quatCopy(&prev_ref, output);

}
Пример #4
0
void rateApplyLocalRotation(Quaternion *rot) {

    Quaternion current_ref;
    
    rgltrGetQuatRef(&current_ref);
    quatMult(&current_ref, rot, &current_ref);
    quatNormalize(&current_ref);
    rgltrSetQuatRef(&current_ref);

}
Пример #5
0
// 12000 cycles?
void attEstimatePose(void) {

    Quaternion displacement_quat;
    float rate[3], norm, sina_2;
    bams32_t a_2;

    if(!is_ready) { return; }
    if(!is_running) { return; }

    gyroGetRadXYZ(rate); // Get last read gyro values
    timestamp = swatchToc(); // Record timestamp

    // Calculate magnitude and disiplacement
    norm = sqrtf(rate[0]*rate[0] + rate[1]*rate[1] + rate[2]*rate[2]);

    // Special case when no movement occurs due to simplification below
    if(norm == 0.0) {

        displacement_quat.w = 1.0;
        displacement_quat.x = 0.0;
        displacement_quat.y = 0.0;
        displacement_quat.z = 0.0;

    } else {

        // Generate displacement rotation quaternion
        // Normally this is w = cos(a/2), but we can delay normalizing
        // by multiplying all terms by norm
        a_2 = floatToBams32Rad(norm*sample_period)/2;
        sina_2 = bams32SinFine(a_2);

        displacement_quat.w = bams32CosFine(a_2)*norm;
        displacement_quat.x = sina_2*rate[0];
        displacement_quat.y = sina_2*rate[1];
        displacement_quat.z = sina_2*rate[2];
        quatNormalize(&displacement_quat);
    }

    // Apply displacement to pose
    quatMult(&pose_quat, &displacement_quat, &pose_quat);

    // Normalize pose quaternion to account for unnormalized displacement quaternion
    quatNormalize(&pose_quat);
    calculateEulerAngles();

}
Пример #6
0
vec4d vec4d::quatDiv(vec4d x) {
  return quatMult(x.quatConj()) * (1 / x.sqnorm());
}
Пример #7
0
void main(void)\n\
{  \n\
   float fTime0_X = parameters[0][0];\n\
   vec4 coreSeed = parameters[1];\n\
\n\
   vec3 rayDir = normalize(objectPosition * vec3(1.0, 0.6, 1.0));\n\
   vec3 camPos = vec3(0.0, 0.0, -parameters[0][1]);\n\
   \n\
   // rotate camera around y axis\n\
   float alpha = parameters[0][2] * 4.5f;\n\
   camPos.xz = vec2(cos(alpha)*camPos.x - sin(alpha)*camPos.z,\n\
                    sin(alpha)*camPos.x + cos(alpha)*camPos.z);\n\
   rayDir.xz = vec2(cos(alpha)*rayDir.x - sin(alpha)*rayDir.z,\n\
                    sin(alpha)*rayDir.x + cos(alpha)*rayDir.z);\n\
   \n\
   vec3 rayPos = camPos;\n\
   float sceneSize = 8.0;\n\
   vec3 totalColor = vec3(0.);\n\
   float stepSize;\n\
   float totalDensity = 0.0;\n\
   float stepDepth = 0.0; // how far I went already.\n\
   \n\
   for(int step = 0; length(rayPos)<sceneSize && totalDensity < 0.9 && step < 50; step++)\n\
   {      \n\
      float implicitVal;\n\
      \n\
      // This stuff is the transformation information from previous stuff\n\
	  float transer = parameters[0][3];\n\
      vec4 prevQuaternion = vec4(cos(transer), sin(transer), sin(transer * 1.3), sin(transer * 2.7));\n\
      prevQuaternion = normalize(prevQuaternion);\n\
      float prevLength = 1.0;\n\
      vec3 prevMover = vec3(0.0);\n\
      vec3 prevColor = vec3(1.0, 0.4, 0.2);\n\
      \n\
      // Multiple boxes\n\
      implicitVal = 1.0e10;\n\
      \n\
      for (int loop = 0; loop < 12; loop++)\n\
      {\n\
         vec4 newQuaternion;\n\
         float newLength;\n\
         vec3 newMover;\n\
         vec3 newColor;\n\
         \n\
         mat3 prevRotationMatrix = quaternionToMatrix(prevQuaternion);\n\
\n\
         // Loop for solid stuff\n\
         vec4 seed = coreSeed;\n\
         for (int k = 0; k < 4; k++)\n\
         {\n\
            seed = randomIteration(seed);\n\
            vec4 quaternion = normalize(seed - vec4(0.5));\n\
            mat3 rotationMatrix = quaternionToMatrix(quatMult(quaternion, prevQuaternion));\n\
            vec3 lengthes = seed.xyz * seed.xyz * seed.xyz * seed.xyz * vec3(0.2) + vec3(0.05);\n\
            lengthes *= prevLength;\n\
            vec3 mover = 0.5*seed.wzx - vec3(0.25);\n\
            mover = (mover * prevRotationMatrix * prevLength) + prevMover;\n\
            float curImplicitVal = getDistance(rotationMatrix, lengthes, mover, rayPos);\n\
            implicitVal = min(implicitVal, curImplicitVal);\n\
         }\n\
         \n\
         // Non-solid:\n\
         float nonSolidDist = 1.0e10;\n\
         for (int k = 0; k < 2; k++)\n\
         {\n\
            seed = randomIteration(seed);\n\
            vec4 quaternion = normalize(seed - vec4(0.5));\n\
            quaternion = quatMult(quaternion, prevQuaternion);\n\
            vec3 lengthes = seed.xyz * vec3(0.3) + vec3(0.25);\n\
            lengthes *= prevLength;\n\
            vec3 mover = 0.5*seed.wzx - vec3(0.25);\n\
            mover = (mover * prevRotationMatrix * prevLength) + prevMover;\n\
            float curImplicitVal = getSphereDistance(lengthes.x, mover, rayPos);\n\
            if (curImplicitVal < nonSolidDist)\n\
            {\n\
               nonSolidDist = curImplicitVal;\n\
               newQuaternion = quaternion;\n\
               newLength = lengthes.x;\n\
               newMover = mover;\n\
               newColor = seed.xyz;\n\
            }\n\
         }\n\
         \n\
         if (nonSolidDist > implicitVal)\n\
         {\n\
            // I will not get closer than where I am now.\n\
            break;\n\
         }\n\
         else\n\
         {\n\
            prevQuaternion = newQuaternion;\n\
            prevLength = newLength;\n\
            prevMover = newMover;\n\
            prevColor = 0.5 * prevColor + 0.5 * newColor;      \n\
         }\n\
      }\n\
      \n\
      // I need to do this distance related to for the DOF!      \n\
      totalColor += vec3(1./50., 1./70., 1./90.) *\n\
                    1.7 / exp(abs(implicitVal*5.) + 0.0) * 1.8;\n\
      totalDensity += 1./ 15. / exp(abs(implicitVal*10.0) + 0.5);\n\
                      // *(1.0 - cos(fTime0_X*3.)*0.5);\n\
      //if (implicitVal < 0.0)\n\
      stepDepth += abs(implicitVal) * 0.95;      \n\
      {\n\
         // TODO: I could make this distance-related, with offset to get the size right?\n\
         float localDensity = implicitVal < 0.0 ? 1.0 : 0.0;\n\
         totalColor = totalColor + (1.-totalDensity) * prevColor * localDensity;\n\
         totalDensity = totalDensity + (1.05-totalDensity) * localDensity;\n\
      }\n\
      \n\
      stepSize = abs(implicitVal) * 0.99;\n\
      stepSize = max(0.005 * stepDepth, stepSize);\n\
      rayPos += rayDir * stepSize;\n\
   }\n\
   \n\
   float grad = normalize(rayDir).y;\n\
   totalColor += (1.-totalDensity) * (grad * vec3(0.0,-0.4,-0.3) + (1.-grad)*vec3(0.0,0.4,0.6));\n\
   \n\
   gl_FragColor = vec4(totalColor-vec3(0.0), 1.0);\n\
}\n\