Examples with Transform spacegraph.space3d.phys.math.Transform used on opensource projects

Example 21 with Transform

use of spacegraph.space3d.phys.math.Transform in project narchy by automenta.

the class TransformStackList method get.

public Transform get(Matrix3f mat) {
    Transform obj = get();
    obj.basis.set(mat);
    obj.set(0f, 0f, 0f);
    return obj;
}

Example 22 with Transform

use of spacegraph.space3d.phys.math.Transform in project narchy by automenta.

the class RagDoll method build.

public Body3D[] build(Dynamics3D world, v3 positionOffset, float scale_ragdoll) {
    stack.pushCommonMath();
    Transform tmpTrans = stack.transforms.get();
    // Setup the geometry
    shapes[BodyPart.BODYPART_PELVIS.ordinal()] = new CapsuleShape(scale_ragdoll * 0.15f, scale_ragdoll * 0.20f);
    shapes[BodyPart.BODYPART_SPINE.ordinal()] = new CapsuleShape(scale_ragdoll * 0.15f, scale_ragdoll * 0.28f);
    shapes[BodyPart.BODYPART_HEAD.ordinal()] = new CapsuleShape(scale_ragdoll * 0.10f, scale_ragdoll * 0.05f);
    shapes[BodyPart.BODYPART_LEFT_UPPER_LEG.ordinal()] = new CapsuleShape(scale_ragdoll * 0.07f, scale_ragdoll * 0.45f);
    shapes[BodyPart.BODYPART_LEFT_LOWER_LEG.ordinal()] = new CapsuleShape(scale_ragdoll * 0.05f, scale_ragdoll * 0.37f);
    shapes[BodyPart.BODYPART_RIGHT_UPPER_LEG.ordinal()] = new CapsuleShape(scale_ragdoll * 0.07f, scale_ragdoll * 0.45f);
    shapes[BodyPart.BODYPART_RIGHT_LOWER_LEG.ordinal()] = new CapsuleShape(scale_ragdoll * 0.05f, scale_ragdoll * 0.37f);
    shapes[BodyPart.BODYPART_LEFT_UPPER_ARM.ordinal()] = new CapsuleShape(scale_ragdoll * 0.05f, scale_ragdoll * 0.33f);
    shapes[BodyPart.BODYPART_LEFT_LOWER_ARM.ordinal()] = new CapsuleShape(scale_ragdoll * 0.04f, scale_ragdoll * 0.25f);
    shapes[BodyPart.BODYPART_RIGHT_UPPER_ARM.ordinal()] = new CapsuleShape(scale_ragdoll * 0.05f, scale_ragdoll * 0.33f);
    shapes[BodyPart.BODYPART_RIGHT_LOWER_ARM.ordinal()] = new CapsuleShape(scale_ragdoll * 0.04f, scale_ragdoll * 0.25f);
    // Setup all the rigid bodies
    Transform offset = stack.transforms.get();
    offset.setIdentity();
    offset.set(positionOffset);
    Transform transform = stack.transforms.get();
    transform.setIdentity();
    transform.set(0f, scale_ragdoll * 1f, 0f);
    tmpTrans.mul(offset, transform);
    bodies[BodyPart.BODYPART_PELVIS.ordinal()] = localCreateRigidBody(1f, tmpTrans, shapes[BodyPart.BODYPART_PELVIS.ordinal()]);
    transform.setIdentity();
    transform.set(0f, scale_ragdoll * 1.2f, 0f);
    tmpTrans.mul(offset, transform);
    bodies[BodyPart.BODYPART_SPINE.ordinal()] = localCreateRigidBody(1f, tmpTrans, shapes[BodyPart.BODYPART_SPINE.ordinal()]);
    transform.setIdentity();
    transform.set(0f, scale_ragdoll * 1.6f, 0f);
    tmpTrans.mul(offset, transform);
    this.head = bodies[BodyPart.BODYPART_HEAD.ordinal()] = localCreateRigidBody(1f, tmpTrans, shapes[BodyPart.BODYPART_HEAD.ordinal()]);
    // head.setRenderer((Object gl, Object dd)->{
    // GL2 g = (GL2) gl;
    // Dynamic d = (Dynamic) dd;
    // 
    // defaultRenderer.accept(g, d); //HACK cast sucks
    // 
    // g.glPushMatrix();
    // 
    // Draw.transform(g, d.transform());
    // 
    // g.glColor4f((float)Math.random(),0.25f,1f,0.75f); //HACK cast sucks
    // g.glRotatef((float)Math.PI/2f, 0, 0, 1);
    // Draw.rect(g, -0.5f,-0.5f,1,1, 0.5f);
    // 
    // g.glPopMatrix();
    // 
    // g.glColor4f(0.75f,0.75f,0.75f,1f); //HACK restore white color
    // 
    // });
    // 
    transform.setIdentity();
    transform.set(-0.18f * scale_ragdoll, 0.65f * scale_ragdoll, 0f);
    tmpTrans.mul(offset, transform);
    bodies[BodyPart.BODYPART_LEFT_UPPER_LEG.ordinal()] = localCreateRigidBody(1f, tmpTrans, shapes[BodyPart.BODYPART_LEFT_UPPER_LEG.ordinal()]);
    transform.setIdentity();
    transform.set(-0.18f * scale_ragdoll, 0.2f * scale_ragdoll, 0f);
    tmpTrans.mul(offset, transform);
    bodies[BodyPart.BODYPART_LEFT_LOWER_LEG.ordinal()] = localCreateRigidBody(1f, tmpTrans, shapes[BodyPart.BODYPART_LEFT_LOWER_LEG.ordinal()]);
    transform.setIdentity();
    transform.set(0.18f * scale_ragdoll, 0.65f * scale_ragdoll, 0f);
    tmpTrans.mul(offset, transform);
    bodies[BodyPart.BODYPART_RIGHT_UPPER_LEG.ordinal()] = localCreateRigidBody(1f, tmpTrans, shapes[BodyPart.BODYPART_RIGHT_UPPER_LEG.ordinal()]);
    transform.setIdentity();
    transform.set(0.18f * scale_ragdoll, 0.2f * scale_ragdoll, 0f);
    tmpTrans.mul(offset, transform);
    bodies[BodyPart.BODYPART_RIGHT_LOWER_LEG.ordinal()] = localCreateRigidBody(1f, tmpTrans, shapes[BodyPart.BODYPART_RIGHT_LOWER_LEG.ordinal()]);
    transform.setIdentity();
    transform.set(-0.35f * scale_ragdoll, 1.45f * scale_ragdoll, 0f);
    MatrixUtil.setEulerZYX(transform.basis, 0, 0, ExtraGlobals.SIMD_HALF_PI);
    tmpTrans.mul(offset, transform);
    bodies[BodyPart.BODYPART_LEFT_UPPER_ARM.ordinal()] = localCreateRigidBody(1f, tmpTrans, shapes[BodyPart.BODYPART_LEFT_UPPER_ARM.ordinal()]);
    transform.setIdentity();
    transform.set(-0.7f * scale_ragdoll, 1.45f * scale_ragdoll, 0f);
    MatrixUtil.setEulerZYX(transform.basis, (float) 0, 0, ExtraGlobals.SIMD_HALF_PI);
    tmpTrans.mul(offset, transform);
    bodies[BodyPart.BODYPART_LEFT_LOWER_ARM.ordinal()] = localCreateRigidBody(1f, tmpTrans, shapes[BodyPart.BODYPART_LEFT_LOWER_ARM.ordinal()]);
    transform.setIdentity();
    transform.set(0.35f * scale_ragdoll, 1.45f * scale_ragdoll, 0f);
    MatrixUtil.setEulerZYX(transform.basis, (float) 0, 0, -ExtraGlobals.SIMD_HALF_PI);
    tmpTrans.mul(offset, transform);
    bodies[BodyPart.BODYPART_RIGHT_UPPER_ARM.ordinal()] = localCreateRigidBody(1f, tmpTrans, shapes[BodyPart.BODYPART_RIGHT_UPPER_ARM.ordinal()]);
    transform.setIdentity();
    transform.set(0.7f * scale_ragdoll, 1.45f * scale_ragdoll, 0f);
    MatrixUtil.setEulerZYX(transform.basis, 0, 0, -ExtraGlobals.SIMD_HALF_PI);
    tmpTrans.mul(offset, transform);
    bodies[BodyPart.BODYPART_RIGHT_LOWER_ARM.ordinal()] = localCreateRigidBody(1f, tmpTrans, shapes[BodyPart.BODYPART_RIGHT_LOWER_ARM.ordinal()]);
    // Setup some damping on the m_bodies
    // for (int i = 0; i < BodyPart.BODYPART_COUNT.ordinal(); ++i) {
    // bodies[i].setDamping(0.05f, 0.85f);
    // bodies[i].setDeactivationTime(0.8f);
    // bodies[i].setSleepingThresholds(1.6f, 2.5f);
    // }
    // /////////////////////////// SETTING THE CONSTRAINTS /////////////////////////////////////////////7777
    // Now setup the constraints
    Transform localA = stack.transforms.get(), localB = stack.transforms.get();
    // / ******* SPINE HEAD ******** ///
    localA.setIdentity();
    localB.setIdentity();
    localA.set(0f, 0.30f * scale_ragdoll, 0f);
    localB.set(0f, -0.14f * scale_ragdoll, 0f);
    boolean useLinearReferenceFrameA = true;
    Generic6DofConstraint joint6DOF = new Generic6DofConstraint(bodies[BodyPart.BODYPART_SPINE.ordinal()], bodies[BodyPart.BODYPART_HEAD.ordinal()], localA, localB, useLinearReferenceFrameA);
    // #ifdef RIGID
    // joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
    // joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
    // #else
    joint6DOF.setAngularLowerLimit(stack.vectors.get(-ExtraGlobals.SIMD_PI * 0.3f, -ExtraGlobals.FLT_EPSILON, -ExtraGlobals.SIMD_PI * 0.3f));
    joint6DOF.setAngularUpperLimit(stack.vectors.get(ExtraGlobals.SIMD_PI * 0.5f, ExtraGlobals.FLT_EPSILON, ExtraGlobals.SIMD_PI * 0.3f));
    // #endif
    joints[JointType.JOINT_SPINE_HEAD.ordinal()] = joint6DOF;
    world.addConstraint(joints[JointType.JOINT_SPINE_HEAD.ordinal()], true);
    // / *************************** ///
    // / ******* LEFT SHOULDER ******** ///
    localA.setIdentity();
    localB.setIdentity();
    localA.set(-0.2f * scale_ragdoll, 0.15f * scale_ragdoll, 0f);
    MatrixUtil.setEulerZYX(localB.basis, ExtraGlobals.SIMD_HALF_PI, (float) 0, -ExtraGlobals.SIMD_HALF_PI);
    localB.set(0f, -0.18f * scale_ragdoll, 0f);
    joint6DOF = new Generic6DofConstraint(bodies[BodyPart.BODYPART_SPINE.ordinal()], bodies[BodyPart.BODYPART_LEFT_UPPER_ARM.ordinal()], localA, localB, useLinearReferenceFrameA);
    // #ifdef RIGID
    // joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
    // joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
    // #else
    joint6DOF.setAngularLowerLimit(stack.vectors.get(-ExtraGlobals.SIMD_PI * 0.8f, -ExtraGlobals.FLT_EPSILON, -ExtraGlobals.SIMD_PI * 0.5f));
    joint6DOF.setAngularUpperLimit(stack.vectors.get(ExtraGlobals.SIMD_PI * 0.8f, ExtraGlobals.FLT_EPSILON, ExtraGlobals.SIMD_PI * 0.5f));
    // #endif
    joints[JointType.JOINT_LEFT_SHOULDER.ordinal()] = joint6DOF;
    world.addConstraint(joints[JointType.JOINT_LEFT_SHOULDER.ordinal()], true);
    // / *************************** ///
    // / ******* RIGHT SHOULDER ******** ///
    localA.setIdentity();
    localB.setIdentity();
    localA.set(0.2f * scale_ragdoll, 0.15f * scale_ragdoll, 0f);
    MatrixUtil.setEulerZYX(localB.basis, 0f, 0f, ExtraGlobals.SIMD_HALF_PI);
    localB.set(0f, -0.18f * scale_ragdoll, 0f);
    joint6DOF = new Generic6DofConstraint(bodies[BodyPart.BODYPART_SPINE.ordinal()], bodies[BodyPart.BODYPART_RIGHT_UPPER_ARM.ordinal()], localA, localB, useLinearReferenceFrameA);
    // #ifdef RIGID
    // joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
    // joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
    // #else
    joint6DOF.setAngularLowerLimit(stack.vectors.get(-ExtraGlobals.SIMD_PI * 0.8f, -ExtraGlobals.SIMD_EPSILON, -ExtraGlobals.SIMD_PI * 0.5f));
    joint6DOF.setAngularUpperLimit(stack.vectors.get(ExtraGlobals.SIMD_PI * 0.8f, ExtraGlobals.SIMD_EPSILON, ExtraGlobals.SIMD_PI * 0.5f));
    // #endif
    joints[JointType.JOINT_RIGHT_SHOULDER.ordinal()] = joint6DOF;
    world.addConstraint(joints[JointType.JOINT_RIGHT_SHOULDER.ordinal()], true);
    // / *************************** ///
    // / ******* LEFT ELBOW ******** ///
    localA.setIdentity();
    localB.setIdentity();
    localA.set(0f, 0.18f * scale_ragdoll, 0f);
    localB.set(0f, -0.14f * scale_ragdoll, 0f);
    joint6DOF = new Generic6DofConstraint(bodies[BodyPart.BODYPART_LEFT_UPPER_ARM.ordinal()], bodies[BodyPart.BODYPART_LEFT_LOWER_ARM.ordinal()], localA, localB, useLinearReferenceFrameA);
    // #ifdef RIGID
    // joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
    // joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
    // #else
    joint6DOF.setAngularLowerLimit(stack.vectors.get(-ExtraGlobals.SIMD_EPSILON, -ExtraGlobals.SIMD_EPSILON, -ExtraGlobals.SIMD_EPSILON));
    joint6DOF.setAngularUpperLimit(stack.vectors.get(ExtraGlobals.SIMD_PI * 0.7f, ExtraGlobals.SIMD_EPSILON, ExtraGlobals.SIMD_EPSILON));
    // #endif
    joints[JointType.JOINT_LEFT_ELBOW.ordinal()] = joint6DOF;
    world.addConstraint(joints[JointType.JOINT_LEFT_ELBOW.ordinal()], true);
    // / *************************** ///
    // / ******* RIGHT ELBOW ******** ///
    localA.setIdentity();
    localB.setIdentity();
    localA.set(0f, 0.18f * scale_ragdoll, 0f);
    localB.set(0f, -0.14f * scale_ragdoll, 0f);
    joint6DOF = new Generic6DofConstraint(bodies[BodyPart.BODYPART_RIGHT_UPPER_ARM.ordinal()], bodies[BodyPart.BODYPART_RIGHT_LOWER_ARM.ordinal()], localA, localB, useLinearReferenceFrameA);
    // #ifdef RIGID
    // joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
    // joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
    // #else
    joint6DOF.setAngularLowerLimit(stack.vectors.get(-ExtraGlobals.SIMD_EPSILON, -ExtraGlobals.SIMD_EPSILON, -ExtraGlobals.SIMD_EPSILON));
    joint6DOF.setAngularUpperLimit(stack.vectors.get(ExtraGlobals.SIMD_PI * 0.7f, ExtraGlobals.SIMD_EPSILON, ExtraGlobals.SIMD_EPSILON));
    // #endif
    joints[JointType.JOINT_RIGHT_ELBOW.ordinal()] = joint6DOF;
    world.addConstraint(joints[JointType.JOINT_RIGHT_ELBOW.ordinal()], true);
    // / *************************** ///
    // / ******* PELVIS ******** ///
    localA.setIdentity();
    localB.setIdentity();
    MatrixUtil.setEulerZYX(localA.basis, (float) 0, ExtraGlobals.SIMD_HALF_PI, 0);
    localA.set(0f, 0.15f * scale_ragdoll, 0f);
    MatrixUtil.setEulerZYX(localB.basis, (float) 0, ExtraGlobals.SIMD_HALF_PI, 0);
    localB.set(0f, -0.15f * scale_ragdoll, 0f);
    joint6DOF = new Generic6DofConstraint(bodies[BodyPart.BODYPART_PELVIS.ordinal()], bodies[BodyPart.BODYPART_SPINE.ordinal()], localA, localB, useLinearReferenceFrameA);
    // #ifdef RIGID
    // joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
    // joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
    // #else
    joint6DOF.setAngularLowerLimit(stack.vectors.get(-ExtraGlobals.SIMD_PI * 0.2f, -ExtraGlobals.SIMD_EPSILON, -ExtraGlobals.SIMD_PI * 0.3f));
    joint6DOF.setAngularUpperLimit(stack.vectors.get(ExtraGlobals.SIMD_PI * 0.2f, ExtraGlobals.SIMD_EPSILON, ExtraGlobals.SIMD_PI * 0.6f));
    // #endif
    joints[JointType.JOINT_PELVIS_SPINE.ordinal()] = joint6DOF;
    world.addConstraint(joints[JointType.JOINT_PELVIS_SPINE.ordinal()], true);
    // / *************************** ///
    // / ******* LEFT HIP ******** ///
    localA.setIdentity();
    localB.setIdentity();
    localA.set(-0.18f * scale_ragdoll, -0.10f * scale_ragdoll, 0f);
    localB.set(0f, 0.225f * scale_ragdoll, 0f);
    joint6DOF = new Generic6DofConstraint(bodies[BodyPart.BODYPART_PELVIS.ordinal()], bodies[BodyPart.BODYPART_LEFT_UPPER_LEG.ordinal()], localA, localB, useLinearReferenceFrameA);
    // #ifdef RIGID
    // joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
    // joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
    // #else
    joint6DOF.setAngularLowerLimit(stack.vectors.get(-ExtraGlobals.SIMD_HALF_PI * 0.5f, -ExtraGlobals.SIMD_EPSILON, -ExtraGlobals.SIMD_EPSILON));
    joint6DOF.setAngularUpperLimit(stack.vectors.get(ExtraGlobals.SIMD_HALF_PI * 0.8f, ExtraGlobals.SIMD_EPSILON, ExtraGlobals.SIMD_HALF_PI * 0.6f));
    // #endif
    joints[JointType.JOINT_LEFT_HIP.ordinal()] = joint6DOF;
    world.addConstraint(joints[JointType.JOINT_LEFT_HIP.ordinal()], true);
    // / *************************** ///
    // / ******* RIGHT HIP ******** ///
    localA.setIdentity();
    localB.setIdentity();
    localA.set(0.18f * scale_ragdoll, -0.10f * scale_ragdoll, 0f);
    localB.set(0f, 0.225f * scale_ragdoll, 0f);
    joint6DOF = new Generic6DofConstraint(bodies[BodyPart.BODYPART_PELVIS.ordinal()], bodies[BodyPart.BODYPART_RIGHT_UPPER_LEG.ordinal()], localA, localB, useLinearReferenceFrameA);
    // #ifdef RIGID
    // joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
    // joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
    // #else
    joint6DOF.setAngularLowerLimit(stack.vectors.get(-ExtraGlobals.SIMD_HALF_PI * 0.5f, -ExtraGlobals.SIMD_EPSILON, -ExtraGlobals.SIMD_HALF_PI * 0.6f));
    joint6DOF.setAngularUpperLimit(stack.vectors.get(ExtraGlobals.SIMD_HALF_PI * 0.8f, ExtraGlobals.SIMD_EPSILON, ExtraGlobals.SIMD_EPSILON));
    // #endif
    joints[JointType.JOINT_RIGHT_HIP.ordinal()] = joint6DOF;
    world.addConstraint(joints[JointType.JOINT_RIGHT_HIP.ordinal()], true);
    // / *************************** ///
    // / ******* LEFT KNEE ******** ///
    localA.setIdentity();
    localB.setIdentity();
    localA.set(0f, -0.225f * scale_ragdoll, 0f);
    localB.set(0f, 0.185f * scale_ragdoll, 0f);
    joint6DOF = new Generic6DofConstraint(bodies[BodyPart.BODYPART_LEFT_UPPER_LEG.ordinal()], bodies[BodyPart.BODYPART_LEFT_LOWER_LEG.ordinal()], localA, localB, useLinearReferenceFrameA);
    // 
    // #ifdef RIGID
    // joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
    // joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
    // #else
    joint6DOF.setAngularLowerLimit(stack.vectors.get(-ExtraGlobals.SIMD_EPSILON, -ExtraGlobals.SIMD_EPSILON, -ExtraGlobals.SIMD_EPSILON));
    joint6DOF.setAngularUpperLimit(stack.vectors.get(ExtraGlobals.SIMD_PI * 0.7f, ExtraGlobals.SIMD_EPSILON, ExtraGlobals.SIMD_EPSILON));
    // #endif
    joints[JointType.JOINT_LEFT_KNEE.ordinal()] = joint6DOF;
    world.addConstraint(joints[JointType.JOINT_LEFT_KNEE.ordinal()], true);
    // / *************************** ///
    // / ******* RIGHT KNEE ******** ///
    localA.setIdentity();
    localB.setIdentity();
    localA.set(0f, -0.225f * scale_ragdoll, 0f);
    localB.set(0f, 0.185f * scale_ragdoll, 0f);
    joint6DOF = new Generic6DofConstraint(bodies[BodyPart.BODYPART_RIGHT_UPPER_LEG.ordinal()], bodies[BodyPart.BODYPART_RIGHT_LOWER_LEG.ordinal()], localA, localB, useLinearReferenceFrameA);
    // #ifdef RIGID
    // joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
    // joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
    // #else
    joint6DOF.setAngularLowerLimit(stack.vectors.get(-ExtraGlobals.SIMD_EPSILON, -ExtraGlobals.SIMD_EPSILON, -ExtraGlobals.SIMD_EPSILON));
    joint6DOF.setAngularUpperLimit(stack.vectors.get(ExtraGlobals.SIMD_PI * 0.7f, ExtraGlobals.SIMD_EPSILON, ExtraGlobals.SIMD_EPSILON));
    // #endif
    joints[JointType.JOINT_RIGHT_KNEE.ordinal()] = joint6DOF;
    world.addConstraint(joints[JointType.JOINT_RIGHT_KNEE.ordinal()], true);
    // / *************************** ///
    stack.popCommonMath();
    return bodies;
}

Example 23 with Transform

use of spacegraph.space3d.phys.math.Transform in project narchy by automenta.

the class SolverBody method writebackVelocity.

public void writebackVelocity(float timeStep) {
    if (invMass != 0f) {
        body.setLinearVelocity(linearVelocity);
        body.setAngularVelocity(angularVelocity);
        // correct the position/orientation based on push/turn recovery
        newTransform.setIdentity();
        // getWorldTransform(new Transform());
        Transform curTrans = body.transform;
        TransformUtil.integrateTransform(curTrans, pushVelocity, turnVelocity, timeStep, newTransform);
        body.transform(newTransform);
    // m_originalBody->setCompanionId(-1);
    }
}

Example 24 with Transform

use of spacegraph.space3d.phys.math.Transform in project narchy by automenta.

the class Cuboid method onTouch.

@Override
public Surface onTouch(Finger finger, Collidable body, ClosestRay r, short[] buttons, SpaceGraphPhys3D space) {
    if (body != null) {
        // rotate to match camera's orientation (billboarding)
        Object d = body.data();
        if (d instanceof SimpleSpatial) {
            SimpleSpatial sd = (SimpleSpatial) d;
        // Quat4f target = Quat4f.angle(-space.camFwd.x, -space.camFwd.y, -space.camFwd.z, 0);
        // Quat4f target = new Quat4f();
        // sd.rotate( -space.camFwd.x, -space.camFwd.y, -space.camFwd.z, 0, 0.2f);
        // com.jogamp.common.util.SyncedRingbuffer
        // Transform bt = body.worldTransform;
        // // TODO somehow use the object's local transformation ? sd.transform().getRotation(...);
        // target.setAngle(
        // space.camFwd.x-bt.x,
        // space.camFwd.y - bt.y,
        // space.camFwd.z -bt.z,
        // (float) Math.PI
        // );
        // 
        // target.normalize();
        // 
        // sd.rotate(target, 0.2f); //new Quat4f());
        // //System.out.println("  : " + sd.transform().getRotation(new Quat4f()));
        }
        // 
        Surface s0 = super.onTouch(finger, body, r, buttons, space);
        if (s0 != null)
            return s0;
    }
    if (front != null) {
        Transform it = Transform.t(transform).inverse();
        v3 localPoint = it.transform(v(r.hitPointWorld));
        if (body != null && body.shape() instanceof SimpleBoxShape) {
            SimpleBoxShape shape = (SimpleBoxShape) body.shape();
            float frontZ = shape.z() / 2;
            float zTolerance = frontZ / 4f;
            if (Util.equals(localPoint.z, frontZ, zTolerance)) {
                // top surface only, ignore sides and back
                this.mousePick = r.hitPointWorld;
                fingered = finger;
                return fingered.on(front, localPoint.x / shape.x() + 0.5f, localPoint.y / shape.y() + 0.5f, buttons);
            // return mouseFront.update(null, localPoint.x, localPoint.y, buttons);
            }
        } else {
            if (fingered != null && fingered.off()) {
                fingered = null;
            }
        }
    }
    return null;
}

Example 25 with Transform

use of spacegraph.space3d.phys.math.Transform in project narchy by automenta.

the class Maze method create.

@Override
protected void create(Dynamics3D world) {
    float dx = 1, dy = 1;
    float y = 0;
    for (boolean[] c : cells) {
        float x = 0;
        for (boolean cc : c) {
            if (cc) {
                Body3D b = Dynamics3D.newBody(// mass
                1f, new BoxShape(0.9f, 0.9f, 0.9f), new Transform(x, y, 0), // group
                +1, // collidesWithOthersLikeThis ? -1 : -1 & ~(+1) //exclude collisions with self
                -1);
                b.setData(this);
                // b.setLinearFactor(1,1,0); //restricts movement to a 2D plane
                b.setDamping(0.9f, 0.5f);
                b.setFriction(0.9f);
                add(b);
            }
            x += dx;
        }
        y += dy;
    }
    CollisionShape groundShape = new BoxShape(v(20f, 20f, 10f));
    Body3D ground = Dynamics3D.newBody(0f, groundShape, new Transform(0, 0, -15), +1, -1);
    ground.setData(this);
    add(ground);
/*ConvexShape blobShape = new BvhTriangleMeshShape(
                new TriangleIndexVertexArray(), true
        );*/
// ConvexShape blobShape =new TetrahedronShapeEx(v(-10,0,0), v(10, 0, 10), v(10, -10, 10), v(-10, -10, 10));
// CollisionShape blobShape = terrain(5, 0.25f, 1, v(5,5,5));
// ConvexHullShape blobShape = hull();
// Dynamic blob = Dynamics.newBody(4f, blobShape, new Motion(), +1, -1);
// blob.setCenterOfMassTransform(new Transform(0, 0, 15f));
// l.add(blob);
// 
// Collections.addAll( l,  new RagDoll().builder(world, v(0,0,20), 3f) );
}