// File: tomfunkhouser[P]gaps/pkgs/R3Shapes/R3Isect.cpp
// Function: gaps::R3Intersects(gaps::R3Ray const&, gaps::R3Cone const&, gaps::R3Point*, gaps::R3Vector*, double*)
RNClassID R3Intersects(const R3Ray& ray, const R3Cone& cone, 
    R3Point *hit_point1, R3Vector *hit_normal1, RNScalar *hit_t1)
{
    // Check if sphere contains ray start point
    RNBoolean start_inside = R3Contains(cone, ray.Start());
    if (start_inside) {
      if (hit_point1) *hit_point1 = ray.Start();
      if (hit_normal1) *hit_normal1 = -ray.Vector();
      if (hit_t1) *hit_t1 = 0;
      return R3_SPAN_CLASS_ID;
    }

    // Transform ray into canonical coordinate system with
    // axis end point at origin and axis going along negative z axis
    R3Ray ray1(ray);
    R3Affine xform(R4identity_matrix);
    xform.Translate(- (cone.Axis().End().Vector()) );
    xform.Rotate(cone.Axis().Vector(), R3negz_vector);
    ray1.Transform(xform);

    // Compute coefficients for quadratic formula
    RNScalar r = cone.Radius();
    const R3Point& p = ray1.Start();
    const R3Vector& v = ray1.Vector();
    RNScalar A = v.X()*v.X() + v.Y()*v.Y() - r*r*v.Z()*v.Z();
    RNScalar B = 2.0*p.X()*v.X() + 2.0*p.Y()*v.Y() - 2.0*r*r*p.Z()*v.Z();
    RNScalar C = p.X()*p.X() + p.Y()*p.Y() - r*r*p.Z()*p.Z();

    // Solve for t1 and t2 on sides
    RNScalar ray_t;
    if (RNIsZero(A)) {
        if (RNIsZero(B)) {
	    // No intersection
	    return RN_NULL_CLASS_ID;
	}
	else {
	    // ???
	    ray_t = -C/B;
	}
    }
    else {
        RNScalar sqr = B*B - 4.0*A*C;
	if (RNIsNegative(sqr)) {
	    // No intersection
	    return RN_NULL_CLASS_ID;
	}
	else if (RNIsPositive(sqr)) {
	    // Intersection at two points
	    RNScalar s = sqrt(sqr);
	    RNScalar ray_t1 = 0.5 * (-B - s) / A;
	    RNScalar ray_t2 = 0.5 * (-B + s) / A;
	    ray_t = (ray_t1 < ray_t2) ? ray_t1 : ray_t2;
	}
	else {
	    // Intersection at one point
	    ray_t = -0.5 * B / A;
	}
    }

    // Check whether intersection point is on right side of ray
    if (RNIsNegative(ray_t)) return RN_NULL_CLASS_ID;

    // Check whether intersection point is on right side of double cone or beyond cap
    R3Point p1 = ray1.Point(ray_t);
    if (RNIsNegative(p1.Z())) return RN_NULL_CLASS_ID;
    if (RNIsGreater(p1.Z(), cone.Axis().Length())) return RN_NULL_CLASS_ID;

    // Check for intersection with base
    if (RNIsGreater(ray1.Start().Z(), cone.Axis().Length())) {
        // ???
    }

    // This is not complete ???

    // Transform intersection back to world coordinates
    R3Point intersection = ray.Point(ray_t);
    intersection.InverseTransform(xform);

    // Ray hit cone
    if (hit_t1) *hit_t1 = ray_t;
    if (hit_point1) { *hit_point1 = intersection; }
    if (hit_normal1) { *hit_normal1 = R3zero_vector; }
    return R3_POINT_CLASS_ID;
}

// Assembly:
subq $0x408, %rsp            # imm = 0x408
movq %rdi, 0x3f8(%rsp)
movq %rsi, 0x3f0(%rsp)
movq %rdx, 0x3e8(%rsp)
movq %rcx, 0x3e0(%rsp)
movq %r8, 0x3d8(%rsp)
movq 0x3f0(%rsp), %rax
movq %rax, 0x1a0(%rsp)
movq 0x3f8(%rsp), %rdi
callq 0x3c0c0
movq 0x1a0(%rsp), %rdi
movq %rax, %rsi
callq 0x86350
movl %eax, 0x3d4(%rsp)
cmpl $0x0, 0x3d4(%rsp)
je 0x7d880
cmpq $0x0, 0x3e8(%rsp)
je 0x7d819
movq 0x3f8(%rsp), %rdi
callq 0x3c0c0
movq %rax, %rsi
movq 0x3e8(%rsp), %rdi
callq 0x6f1e0
cmpq $0x0, 0x3e0(%rsp)
je 0x7d856
movq 0x3f8(%rsp), %rdi
callq 0x3c0e0
movq %rax, %rsi
leaq 0x3b8(%rsp), %rdi
callq 0x70fa0
movq 0x3e0(%rsp), %rdi
leaq 0x3b8(%rsp), %rsi
callq 0x70970
cmpq $0x0, 0x3d8(%rsp)
je 0x7d870
movq 0x3d8(%rsp), %rax
xorps %xmm0, %xmm0
movsd %xmm0, (%rax)
movl $0x4, 0x404(%rsp)
jmp 0x7e2a9
movq 0x3f8(%rsp), %rsi
leaq 0x388(%rsp), %rdi
callq 0x6e3e0
leaq 0xfb394(%rip), %rsi     # 0x178c30
leaq 0x278(%rsp), %rdi
xorl %edx, %edx
callq 0x4d390
movq 0x3f0(%rsp), %rdi
callq 0x81e60
movq %rax, 0x198(%rsp)
jmp 0x7d8c2
movq 0x198(%rsp), %rdi
callq 0x39260
movq %rax, 0x190(%rsp)
jmp 0x7d8d9
movq 0x190(%rsp), %rsi
leaq 0x248(%rsp), %rdi
callq 0x36d10
jmp 0x7d8f0
leaq 0x260(%rsp), %rdi
leaq 0x248(%rsp), %rsi
callq 0x70fa0
jmp 0x7d907
leaq 0x278(%rsp), %rdi
leaq 0x260(%rsp), %rsi
callq 0x36cd0
jmp 0x7d91e
movq 0x3f0(%rsp), %rdi
callq 0x81e60
movq %rax, 0x188(%rsp)
jmp 0x7d935
movq 0x188(%rsp), %rdi
callq 0x392b0
movq %rax, 0x180(%rsp)
jmp 0x7d94c
movq 0x180(%rsp), %rsi
leaq 0x4fbced(%rip), %rdx    # 0x579648
leaq 0x278(%rsp), %rdi
callq 0x39270
jmp 0x7d96a
leaq 0x388(%rsp), %rdi
leaq 0x278(%rsp), %rsi
callq 0x6e600
jmp 0x7d981
movq 0x3f0(%rsp), %rdi
callq 0x78cd0
movsd %xmm0, 0x178(%rsp)
jmp 0x7d999
movsd 0x178(%rsp), %xmm0
movsd %xmm0, 0x230(%rsp)
leaq 0x388(%rsp), %rdi
callq 0x3c0c0
movq %rax, 0x170(%rsp)
jmp 0x7d9c2
movq 0x170(%rsp), %rax
movq %rax, 0x228(%rsp)
leaq 0x388(%rsp), %rdi
callq 0x3c0e0
movq %rax, 0x168(%rsp)
jmp 0x7d9e9
movq 0x168(%rsp), %rax
movq %rax, 0x220(%rsp)
movq 0x220(%rsp), %rdi
callq 0x38c20
movsd %xmm0, 0x160(%rsp)
jmp 0x7da11
movq 0x220(%rsp), %rdi
callq 0x38c20
movsd %xmm0, 0x158(%rsp)
jmp 0x7da29
movq 0x220(%rsp), %rdi
callq 0x38c30
movsd %xmm0, 0x150(%rsp)
jmp 0x7da41
movq 0x220(%rsp), %rdi
callq 0x38c30
movsd %xmm0, 0x148(%rsp)
jmp 0x7da59
movsd 0x158(%rsp), %xmm2
movsd 0x160(%rsp), %xmm0
movsd 0x148(%rsp), %xmm3
movsd 0x150(%rsp), %xmm1
mulsd %xmm3, %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x130(%rsp)
movsd 0x230(%rsp), %xmm0
mulsd %xmm0, %xmm0
movsd %xmm0, 0x138(%rsp)
movq 0x220(%rsp), %rdi
callq 0x38c40
movsd %xmm0, 0x140(%rsp)
jmp 0x7dac0
movsd 0x140(%rsp), %xmm1
movsd 0x138(%rsp), %xmm0
mulsd %xmm1, %xmm0
movsd %xmm0, 0x120(%rsp)
movq 0x220(%rsp), %rdi
callq 0x38c40
movsd %xmm0, 0x128(%rsp)
jmp 0x7daf7
movsd 0x130(%rsp), %xmm0
movsd 0x128(%rsp), %xmm2
movsd 0x120(%rsp), %xmm1
mulsd %xmm2, %xmm1
subsd %xmm1, %xmm0
movsd %xmm0, 0x218(%rsp)
movq 0x228(%rsp), %rdi
callq 0x37820
movsd %xmm0, 0x118(%rsp)
jmp 0x7db3b
movsd 0x118(%rsp), %xmm0
addsd %xmm0, %xmm0
movsd %xmm0, 0x108(%rsp)
movq 0x220(%rsp), %rdi
callq 0x38c20
movsd %xmm0, 0x110(%rsp)
jmp 0x7db69
movq 0x228(%rsp), %rdi
callq 0x37870
movsd %xmm0, 0x100(%rsp)
jmp 0x7db81
movsd 0x100(%rsp), %xmm0
addsd %xmm0, %xmm0
movsd %xmm0, 0xf0(%rsp)
movq 0x220(%rsp), %rdi
callq 0x38c30
movsd %xmm0, 0xf8(%rsp)
jmp 0x7dbaf
movsd 0x110(%rsp), %xmm2
movsd 0x108(%rsp), %xmm0
movsd 0xf8(%rsp), %xmm3
movsd 0xf0(%rsp), %xmm1
mulsd %xmm3, %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0xd8(%rsp)
movsd 0x230(%rsp), %xmm1
movaps %xmm1, %xmm0
addsd %xmm0, %xmm0
mulsd %xmm1, %xmm0
movsd %xmm0, 0xe0(%rsp)
movq 0x228(%rsp), %rdi
callq 0x378c0
movsd %xmm0, 0xe8(%rsp)
jmp 0x7dc1d
movsd 0xe8(%rsp), %xmm1
movsd 0xe0(%rsp), %xmm0
mulsd %xmm1, %xmm0
movsd %xmm0, 0xc8(%rsp)
movq 0x220(%rsp), %rdi
callq 0x38c40
movsd %xmm0, 0xd0(%rsp)
jmp 0x7dc54
movsd 0xd8(%rsp), %xmm0
movsd 0xd0(%rsp), %xmm2
movsd 0xc8(%rsp), %xmm1
mulsd %xmm2, %xmm1
subsd %xmm1, %xmm0
movsd %xmm0, 0x210(%rsp)
movq 0x228(%rsp), %rdi
callq 0x37820
movsd %xmm0, 0xc0(%rsp)
jmp 0x7dc98
movq 0x228(%rsp), %rdi
callq 0x37820
movsd %xmm0, 0xb8(%rsp)
jmp 0x7dcb0
movq 0x228(%rsp), %rdi
callq 0x37870
movsd %xmm0, 0xb0(%rsp)
jmp 0x7dcc8
movq 0x228(%rsp), %rdi
callq 0x37870
movsd %xmm0, 0xa8(%rsp)
jmp 0x7dce0
movsd 0xb8(%rsp), %xmm2
movsd 0xc0(%rsp), %xmm0
movsd 0xa8(%rsp), %xmm3
movsd 0xb0(%rsp), %xmm1
mulsd %xmm3, %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x90(%rsp)
movsd 0x230(%rsp), %xmm0
mulsd %xmm0, %xmm0
movsd %xmm0, 0x98(%rsp)
movq 0x228(%rsp), %rdi
callq 0x378c0
movsd %xmm0, 0xa0(%rsp)
jmp 0x7dd47
movsd 0xa0(%rsp), %xmm1
movsd 0x98(%rsp), %xmm0
mulsd %xmm1, %xmm0
movsd %xmm0, 0x80(%rsp)
movq 0x228(%rsp), %rdi
callq 0x378c0
movsd %xmm0, 0x88(%rsp)
jmp 0x7dd7e
movsd 0x90(%rsp), %xmm0
movsd 0x88(%rsp), %xmm2
movsd 0x80(%rsp), %xmm1
mulsd %xmm2, %xmm1
subsd %xmm1, %xmm0
movsd %xmm0, 0x208(%rsp)
movsd 0x218(%rsp), %xmm0
leaq 0xfa726(%rip), %rax     # 0x1784e0
movsd (%rax), %xmm1
callq 0x36b80
movl %eax, 0x7c(%rsp)
jmp 0x7ddc9
movl 0x7c(%rsp), %eax
cmpl $0x0, %eax
je 0x7de76
movsd 0x210(%rsp), %xmm0
leaq 0xfa6fa(%rip), %rax     # 0x1784e0
movsd (%rax), %xmm1
callq 0x36b80
movl %eax, 0x78(%rsp)
jmp 0x7ddf5
movl 0x78(%rsp), %eax
cmpl $0x0, %eax
je 0x7de3f
movl $0x0, 0x404(%rsp)
movl $0x1, 0x1fc(%rsp)
jmp 0x7e29c
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x240(%rsp)
movl %eax, 0x23c(%rsp)
leaq 0x278(%rsp), %rdi
callq 0x4d430
jmp 0x7e2b8
movsd 0x208(%rsp), %xmm0
movq %xmm0, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm0
divsd 0x210(%rsp), %xmm0
movsd %xmm0, 0x200(%rsp)
jmp 0x7e036
movsd 0x210(%rsp), %xmm0
movsd 0x218(%rsp), %xmm1
movsd 0xb7ac8(%rip), %xmm2    # 0x135958
mulsd %xmm2, %xmm1
movsd 0x208(%rsp), %xmm2
mulsd %xmm2, %xmm1
mulsd %xmm0, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x1f0(%rsp)
movsd 0x1f0(%rsp), %xmm0
leaq 0xfa61e(%rip), %rax     # 0x1784e0
movsd (%rax), %xmm1
callq 0x38fd0
movl %eax, 0x74(%rsp)
jmp 0x7ded1
movl 0x74(%rsp), %eax
cmpl $0x0, %eax
je 0x7def5
movl $0x0, 0x404(%rsp)
movl $0x1, 0x1fc(%rsp)
jmp 0x7e29c
movsd 0x1f0(%rsp), %xmm0
leaq 0xfa5db(%rip), %rax     # 0x1784e0
movsd (%rax), %xmm1
callq 0x36d50
movl %eax, 0x70(%rsp)
jmp 0x7df14
movl 0x70(%rsp), %eax
cmpl $0x0, %eax
je 0x7e00f
movsd 0x1f0(%rsp), %xmm0
callq 0xf390
movsd %xmm0, 0x1e8(%rsp)
movsd 0x210(%rsp), %xmm0
movq %xmm0, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm1
subsd 0x1e8(%rsp), %xmm1
movsd 0xb50a7(%rip), %xmm0    # 0x133010
mulsd %xmm1, %xmm0
divsd 0x218(%rsp), %xmm0
movsd %xmm0, 0x1e0(%rsp)
movsd 0x210(%rsp), %xmm0
movq %xmm0, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm1
addsd 0x1e8(%rsp), %xmm1
movsd 0xb5060(%rip), %xmm0    # 0x133010
mulsd %xmm1, %xmm0
divsd 0x218(%rsp), %xmm0
movsd %xmm0, 0x1d8(%rsp)
movsd 0x1e0(%rsp), %xmm1
movsd 0x1d8(%rsp), %xmm0
ucomisd %xmm1, %xmm0
jbe 0x7dfef
movsd 0x1e0(%rsp), %xmm0
movsd %xmm0, 0x68(%rsp)
jmp 0x7dffe
movsd 0x1d8(%rsp), %xmm0
movsd %xmm0, 0x68(%rsp)
movsd 0x68(%rsp), %xmm0
movsd %xmm0, 0x200(%rsp)
jmp 0x7e032
movsd 0xb5249(%rip), %xmm0    # 0x133260
mulsd 0x210(%rsp), %xmm0
divsd 0x218(%rsp), %xmm0
movsd %xmm0, 0x200(%rsp)
jmp 0x7e034
jmp 0x7e036
movsd 0x200(%rsp), %xmm0
leaq 0xfa49a(%rip), %rax     # 0x1784e0
movsd (%rax), %xmm1
callq 0x38fd0
movl %eax, 0x64(%rsp)
jmp 0x7e055
movl 0x64(%rsp), %eax
cmpl $0x0, %eax
je 0x7e079
movl $0x0, 0x404(%rsp)
movl $0x1, 0x1fc(%rsp)
jmp 0x7e29c
movsd 0x200(%rsp), %xmm0
leaq 0x1c0(%rsp), %rdi
leaq 0x388(%rsp), %rsi
callq 0x6e4e0
jmp 0x7e099
leaq 0x1c0(%rsp), %rdi
callq 0x378c0
movsd %xmm0, 0x58(%rsp)
jmp 0x7e0ae
movsd 0x58(%rsp), %xmm0
leaq 0xfa425(%rip), %rax     # 0x1784e0
movsd (%rax), %xmm1
callq 0x38fd0
movl %eax, 0x54(%rsp)
jmp 0x7e0ca
movl 0x54(%rsp), %eax
cmpl $0x0, %eax
je 0x7e0ee
movl $0x0, 0x404(%rsp)
movl $0x1, 0x1fc(%rsp)
jmp 0x7e29c
leaq 0x1c0(%rsp), %rdi
callq 0x378c0
movsd %xmm0, 0x48(%rsp)
jmp 0x7e103
movq 0x3f0(%rsp), %rdi
callq 0x81e60
movq %rax, 0x40(%rsp)
jmp 0x7e117
movq 0x40(%rsp), %rdi
callq 0x37a50
movsd %xmm0, 0x38(%rsp)
jmp 0x7e129
movsd 0x38(%rsp), %xmm1
movsd 0x48(%rsp), %xmm0
leaq 0xfa3a4(%rip), %rax     # 0x1784e0
movsd (%rax), %xmm2
callq 0x3c090
movl %eax, 0x34(%rsp)
jmp 0x7e14b
movl 0x34(%rsp), %eax
cmpl $0x0, %eax
je 0x7e16f
movl $0x0, 0x404(%rsp)
movl $0x1, 0x1fc(%rsp)
jmp 0x7e29c
leaq 0x388(%rsp), %rdi
callq 0x3c0c0
movq %rax, 0x28(%rsp)
jmp 0x7e183
movq 0x28(%rsp), %rdi
callq 0x378c0
movsd %xmm0, 0x20(%rsp)
jmp 0x7e195
movq 0x3f0(%rsp), %rdi
callq 0x81e60
movq %rax, 0x18(%rsp)
jmp 0x7e1a9
movq 0x18(%rsp), %rdi
callq 0x37a50
movsd %xmm0, 0x10(%rsp)
jmp 0x7e1bb
movsd 0x10(%rsp), %xmm1
movsd 0x20(%rsp), %xmm0
leaq 0xfa312(%rip), %rax     # 0x1784e0
movsd (%rax), %xmm2
callq 0x3c090
movl %eax, 0xc(%rsp)
jmp 0x7e1dd
movl 0xc(%rsp), %eax
cmpl $0x0, %eax
je 0x7e1e8
jmp 0x7e1e8
movq 0x3f8(%rsp), %rsi
movsd 0x200(%rsp), %xmm0
leaq 0x1a8(%rsp), %rdi
callq 0x6e4e0
jmp 0x7e208
leaq 0x1a8(%rsp), %rdi
leaq 0x278(%rsp), %rsi
callq 0x6f7f0
jmp 0x7e21f
cmpq $0x0, 0x3d8(%rsp)
je 0x7e23f
movsd 0x200(%rsp), %xmm0
movq 0x3d8(%rsp), %rax
movsd %xmm0, (%rax)
cmpq $0x0, 0x3e8(%rsp)
je 0x7e263
movq 0x3e8(%rsp), %rdi
leaq 0x1a8(%rsp), %rsi
callq 0x6f1e0
jmp 0x7e261
jmp 0x7e263
cmpq $0x0, 0x3e0(%rsp)
je 0x7e286
movq 0x3e0(%rsp), %rdi
leaq 0x4fb323(%rip), %rsi    # 0x5795a0
callq 0x70970
jmp 0x7e284
jmp 0x7e286
movl $0x1, 0x404(%rsp)
movl $0x1, 0x1fc(%rsp)
leaq 0x278(%rsp), %rdi
callq 0x4d430
movl 0x404(%rsp), %eax
addq $0x408, %rsp            # imm = 0x408
retq
movq 0x240(%rsp), %rdi
callq 0xf890
nopw %cs:(%rax,%rax)